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ABSTRACT 


An  application  of  two-dimensional  Fourier  Transform  correlation 
for  the  attitude  measurement  of  three-dimensional  objects  is  considered. 
Included  are  geometric  optical  equations  and  geometric  translation  and 
rotation  equations  necessary  for  the  image  error  calculations.  Mini¬ 
mum  attitude  resolution  equations  are  developed  to  estimate  image 
quantization  necessary  for  a  given  measurement  error.  These  techniques 
are  then  applied  as  an  additional  measurement  to  the  radar  tracking 
problem. 
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I .  INTRODUCTION 


The  development  of  sophisticated  naval  weapons  has  resulted  In  the 
requirement  for  high  accuracy  measurement  tests  and  discrete  analysis  of 
complex  system  parameters.  Typical  of  this  precise  test  and  evaluation 
requirement  Is  attitude  measurement  of  three-dimensional  airborne  objects 
by  processing  multiple  two-dimensional  images. 

The  most  common  method  used  to  estimate  attitude  is  by  making 
absolute  or  precisely  relative  three-dimensional  geometric  trajectory 
measurements  of  target  object  extreme  structural  features  (nose,  tail, 
right  wing  tip,  left  wing  tip,  etc.)  from  camera  images.  By  appropriate 
camera,  range  coordinate  system,  and  target  coordinate  system  trans¬ 
formations  these  measurements  will  yield  an  attitude  measurement. 

A  more  direct  method  of  attitude  measurement  involves  the  correlation, 
or  comparison,  of  an  image  of  an  object  at  an  unknown  attitude  with  a 
second  image  of  a  similar  object  at  a  known  attitude.  At  the  Naval 
Weapons  Center,  an  operator  controlled  film  and  television  correlation 
assessment  technique  (FATCAT)  facility  use  this  more  direct  technique 
for  attitude  estimation  [1]. 

This  thesis  presents  an  enhancement  of  the  correlation/comparison 
method  of  attitude  measurement  by  fully  automating  the  technique  incorp¬ 
orating  a  pattern  recognition  method  of  the  directly  digitized  image. 

Image  theory,  resolution,  and  filtering  leading  to  the  two  dimensional 
matched  filter  attitude  measurement  concept  is  discussed  and  this  concept 
is  applied  as  an  additional  measurement  to  a  radar  tracking  problem. 
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II.  IMAGE  THEORY 

Three-dimensional  to  two-dimensional  projections  are  obtained  by 
viewing  the  real  world  as  recorded  by  a  camera  or  other  imaging  device. 
Image  theory  defines  projection  as  the  transformation  of  each  point  in 
the  field  of  view,  through  a  focal  point,  and  onto  a  plane.  This  is 
also  equivalent  to  locating  the  focal  plane  in  front  of  the  focal  point 
and  changing  the  sign  of  the  projected  points  [2],  These  geometrical 
relationships  are  illustrated  in  Fig.  1.  The  transformation  of  point 
x,  y,  z  in  the  X,  Y,  Z  coordinate  system  of  Fig.  1  is  given  by  the 
following  equations: 


f 

- x  (1) 

y  +  f 


f 

V  » - z  .  (2) 

y  +  f 


Where  u  =  projected  x  dimension 
v  =  projected  z  dimension 


If  f  <<  y»  then  the  following  approximations  hold: 


fx 

u  =  — —  =  kx  (3) 


v  = 


(4) 
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Therefore,  for  a  given  focal  length  f,  and  target  range  y,  a  constant 
image  scaling  factor  k,  is  calculated. 


A  second  coordinate  system  X',  V,  V  can  be  used  to  describe  a 
three-dimensional  object  in  the  viewing  space.  If  the  origins  of  these 
two  coordinate  systems  are  coincident,  and  if  all  the  axes  are  parallel, 
then  the  transformation  of  points  from  one  coordinate  system  to  the 
other  would  be  simple. 


(5) 
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If  the  axes  of  the  two  coordinate  systems  are  rotated  by  an  elevation 
angle  9  around  the  camera  Y  axis,  by  an  azimuth  angle  P  around  the  camera 
Z  axis,  and  by  roll  angle  $  around  the  camera  X  axis,  the  transformation 
equation  would  be  [3],  [4]: 


V 

COS  0 

0 

sin  0* 

COS  ip 

-sin  p 

o" 

"l 

0 

0 

X 

y' 

- 

0 

1 

0 

sin  p 

COS  p 

0 

0 

COS  <p 

-sin  p 

y 

(6) 

z' 

-sine 

L 

0 

cos  e_ 

0 

0 

1_ 

_0 

sin  4> 

cos  p 

z 

' 

If  in  addition  there  was  a  translation  of  (A,B,C)  of  the  origin  of  the 
object  coordinate  system  relative  to  the  origin  of  the  camera  coordinate 


system,  the  transformation  equation  would  be: 


V 

•  m 

x’ 

A* 

y' 

= 

0 

<P 

y 

+ 

B 

z' 

»  m 

■  m 

*  « 

z 

-  « 

_C_ 

where 


cos  0 

0 

sin  e 

0 

= 

0 

1 

0 

-  « 

^-sin  e 

0 

COS  0 

- 

“  - 

COS  p 

■sin  p 

0  “ 

P 

= 

sin  p 

cos  P 

0 

- 

0 

0 

1 

- 

’l  0 

0 

♦ 

= 

0  cos 

p 

-sin  <f> 

- 

^0  si n  ^ 

COS  P 
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III.  IMAGE  RESOLUTION 


The  three-dimensional  object  can  be  considered  to  be  an  aircraft, 
with  maximum  orthogonal  dimensions  on  the  focal  plane  of  Xmax,  Ymax, 

Zmax.  These  dimensions  are  illustrated  on  the  top  and  side  views  of 
Fig.  2.  Since  the  image  is  to  be  digitaized,  the  discrete  maximum 
dimensions  can  be  given  in  terms  of  pixel  sampling  intervals,  x  and  y. 

A  pixel  is  defined  as  the  smallest  unit  sampling  area  (x,y)  of  resolution 
for  the  digitized  image. 

Xmax(quantized)  =  XN  (8) 

Ymax (quantized)  =  YN  (9) 

Zmax(quantized)  -  ZN  (10) 

These  dimensions  are  illustrated  in  Fig.  3.  The  quantization  of  the 
image  would  have  a  minimum  rotation  that  could  be  detected  by  a  1  pixel 
change  at  extreme  points  in  the  image  and  would  be  equivalent  to  the 
following  angle: 

tan  e  =  -4r  01) 


This  relationship  is  illustrated  in  Fig.  4.  KN  would  be  equal  to  XN,  YN, 
or  ZN  depending  on  the  angle  of  rotation. 
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Figure  2 

Maximum  Image  Dimensions,  Orthogonal  Views 
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□  =  1  pixel 


Figure  3 

Discrete,  Orthogonal  Views 
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Pivot  Point 


Figure  4 

Minimum  Detectable  Rotation,  Orthogonal  View 


If  the  axis  of  rotation  was  not  orthogonal  to  the  focal  plane,  the 
minimum  detectable  rotation  of  an  equal  sized  object  would  be  much 
larger,  as  indicated  in  the  top  view  of  Fig.  5. 


ccs  9 

min 


KN-1 


KN 


KN  = 


1-COS  9 


mm 


(12) 

(13) 


Sample  values  of  KN  from  equations  (11)  and  (13)  for  minimum  detectable 
rotation  are  given  in  Table  1. 


Side  View  Geometry 


Figure  5.  Minimum  Detectable  Rotation 

Rotation  Axis  in  Viewing  Plane 


5  pixel 


pixel 
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TABLE  1 


9  (degrees) 


Axis  Orthogonal 
to  image  plane  (11) 
KN  (pixels) 


Axis  Parallel 
to  image  plane  (13) 
KN  (pixels) 


1 

58 

6566 

5 

12 

263 

10 

6 

56 

If  it  is  assumed  that  the  rotational  angles  are  measured  by  lines 
through  the  two  extreme  pixels,  the  geometry  and  error  sensitivity  would 
be  as  shown  in  Fig.  6.  For  a  maximum  length  of  KN,  and  a  1  pixel  rota¬ 
tion: 


1 

tan  e  =  - -  (11 ) 

KN 


For  horizontal  errors  H: 


1 

0  =  arctan  - + -  (14) 

KN  -  H 


1 

de  =  d  arctan  - 1 -  (15) 

KN  -  H 


For  a  change  of  variables: 


1 

m  =  -  (16) 

KN  +H 
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V 


m 


dm  = 


dH 

(KN  t  H)2 


(17) 


1 

de  *  - -  dm 

1  +  nr 

dH 

de  -  - x — 

(KN  -  H)2+l 


(18) 

(19) 


For  example,  if  KN  =  58,  H  =  1,  then  de/dH  =  0,0003077  radians/pixel  = 
0.01763  degrees/pixel.  This  sensitivity  for  horizontal  errors  is  very 
small,  as  expected. 

For  vertical  errors: 
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*  arctan 


KN 


m  = 


dm  = 


KN 

dV 

KN 


dV  KN 

de  -  2  +  7 

kn^  +  (i  -  vr 


(20) 

(21) 

(22) 

(23) 


For  example,  if  V  =  1  pixel,  and  KN  =  58  pixels,  de/dV  =  0.01722  radians/ 
pixel  =  0.99  degrees/pixel.  This  is  much  larger  than  the  horizontal 
error,  as  might  be  expected  by  observing  the  geometry  of  Fig.  6. 
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This  section  has  derived  equations  (11)  and  (12)  to  predict  the 


minimum  detectable  rotation  of  a  three-dimensional  object.  These 
equations  apply  to  a  three-dimensional  object  with  a  two-dimensional 
maximum  projected  dimension  of  KN  pixels.  Equation  (11)  applies  if 
the  axis  of  rotation  of  the  three-dimensional  object  is  parallel 
with  the  camera  axis.  Equation  (13)  applies  if  the  axis  of  rotation 
of  the  three-dimensional  object  is  orthogonal  to  the  camera  axis. 
Equations  (19)  and  (23)  predict  the  rotational  errors  resulting  from 
horizontal  and  vertical  measurement  errors  in  extreme  points  of  the 
two-dimensional  image. 


IV.  IMAGE  FILTERING 


Two-dimensional  matched  filtering  can  be  considered  to  be  an 
extension  of  one-dimensional  correlation  filtering  [5].  The  out¬ 
put  of  the  two-dimensional  matched  filter  will  not  be  a  transformed 
image  with  some  type  of  improvement  or  enhancement,  but  a  correlation 
plane  whose  amplitude  at  a  given  location  corresponds  to  the  degree 
of  correlation  of  the  input  image  (signal  +  noise)  with  the  desired 
image  (signal).  This  is  equivalent  to  sliding  one  photographic 
transparency  across  another  and  adding  up  each  black  point  that  was 
aligned  with  another  black  point  on  the  second  image,  each  white 
point  that  was  aligned  with  another  white  point  on  the  second  image, 
and  each  grey  point  that  was  aligned  with  another  grey  point  of 
equal  shade  on  the  second  image. 

Several  practical  factors  must  be  considered  in  the  actual 
simulation  of  the  matched  filter.  The  input  image  and  the  refer¬ 
ence  or  stored  image  could  both  be  compared  (correlated)  as  contin¬ 
uous  grey  scale  images,  but  this  would  be  a  tremendous  storage  and 
computational  task.  Therefore,  the  reference  image  in  the  computer 
simulation  is  digitized  and  stored  as  discrete  X,Y,Z  points  (FACIT 
Model)  in  a  three-dimensional  coordinate  system  [6].  It  is  recon¬ 
structed,  after  rotation  and  translation,  as  unit  amplitude  lines 
projected  on  the  focal  plane.  It  is  also  assumed  that  f  «  y,  and 
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f  and  y  of  equations  (1)  and  (2)  are  known.  This  knowledge  of 
focal  length  and  target  range  yields  a  constant  image  scaling  fac¬ 
tor  (k).  This  type  of  projection  is  illustrated  in  Fig.  7.  An 
example  of  an  aircraft  FACIT  Model  is  given  in  Fig.  8. 

The  input  grey-level  image  is  also  transformed  into  a  similar 
format  by  use  of  an  edge  detector  to  extract  outline  information 
[7].'  The  edge  of  the  image  is  defined  as  the  boundary  between  the 
two  regions  of  different  grey  level. 

The  reference  image  and  the  transformed  input  image  are  applied 
to  a  matched  filter  to  determine  the  degree  of  correlation.  An 
ideal  correlation  function  C(x,y,k,0,^,4>)  would  have  an  impulse 
response  that  could  be  modeled  by  delta,  (sin  x/x),  triangle  or 
exponential  functions.  The  impulse  would  occur  when  the  reference 
and  transformed  images  were: 

1.  aligned  on  the  image  plane  (x,y), 

2.  equally  scaled  (k)  and 

3.  at  the  same  attitude  (e,^)- 

However,  due  to  the  presence  of  noise  and  the  approximation  of  the 
three-dimensional  object  with  a  finite  element  model,  the  correla¬ 
tion  response  is  not  delta,  (sin  x)/x,  triangle  or  exponential  in 
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Figure  7. 

Three-Dimension  to  Two-Dimension  Projection 
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Figure  8.  FACIT  Model  of  F-102 


shape.  It  is  not  necessary  to  know  the  e.xact  correlation  response 
function  or  even  the  constant  component  due  to  a  nonzero  mean  value 
of  the  reference  and  input  image  variable  function.  It  is  only 
necessary  to  know  the  values  of  the  variables  x,  y,  k,  e  ,  ^  and  $ 
at  peak  correlation. 

The  following  simplifying  assumptions  are  made  for  an  attitude 
(6,  <!>,$)  only  correlation: 

1.  the  images  have  been  scaled  to  an  equal  size  (k) 

2.  the  peak  correlation  on  the  image  plane  will  indicate 
translational  alignment  (x,y) 

Fig.  9  illustrates  a  typical  x-y  correlation  between  a  noisy  image 
and  computer-generated  reference  image.  The  peak  correlation  at 
(0.0,  0.0)  pixels  indicates  that  both  images  are  aligned  in  the 
x-y  plane.  This  is  because  the  noisy  image  was  generated  from  a 
computer  reference  image. 

The  attitude  of  the  input  image  is  unknown.  Therefore,  the 
measurement  of  the  attitude  of  the  input  image  is  equivalent  to 
applying  the  transformed  image  to  a  bank  of  matched  filters  with  each 
"tuned"  to  a  specific  attitude.  This  process  is  illustrated  in 
Fig.  10. 
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Figure  10.  Image  Processing  Flow  Diagram 


If  there  was  a  priori  knowledge  of  azimuth  0  ,  elevation  ,  and 
roll  4>  ,  to  within  10  degrees  each,  and  a  1  degree  resolution  of 
measurement  was  required,  a  brute  force  approach  would  require 
10  x  10  x  10  =  1000  correlations  or  1000  matched  filters.  Three  axis 
quadratic  interpolation  is  used  to  reduce  the  number  of  correlations 
required  for  the  given  resolution  of  measurement  [3]. 

It  is  assumed  that  the  variables  azimuth,  elevation,  and  roll 
are  uncoupled  in  the  joint  correlation  function  C(x,y,0,^  4>).  Assum¬ 
ing  that  there  is  a  near  orthogonal  relationship  allows  C(x,y,e  ,<|» ,<j> ) 
to  be  sequentially  maximized  (minimized)  with  the  independent  vari¬ 
ables:  x,  y,  azimuth,  elevation,  and  roll. 
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The  pitch  correlation  function  is  modeled  as  a  quadratic  func¬ 
tion  C  (9) : 


C(0)  =a92+be+c  (24) 

»  2  a  0  +  b  (25) 


Equation  (25)  is  set  to  zero  to  maximize  (minimize)  C(o). 


0 


max 


b 

2  a 


(26) 


9i  =  0  degrees,  02  =  ©i  +  &0  degrees,  and  03  =  @i  -  Ae  degrees  are 
substituted  in  equation  (24)  to  solve  for  the  unknowns  a,  b,  and  c. 
The  solution  of  equation  (26)  is: 


0 


max 


(C(0,)  -  C 


2C(02j  +  2C 


07))  A 9 

e3)  -  4C ( 0 1 ) 


(27) 


Similar  equations  are  written  for  C(i|0,  and  C ( <f> ) . 

The  computer  simulations  listed  in  Appendix  B  implement  equa- 
ion  (27).  The  three  correlations  C (9 1 ) ,  C(02)  and  C(03)  are  calculated 
with  a 9*5  degrees  for  a  rough  approximation  of  emji  .  Then  three 

fuaX 
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correlations  C(© 1  ),  C(02  )  and  0(63  )  are  calculated  with  A0  =  2 


degrees  for  a  sharper  quadratic  approximation  of  emax  .  In  addition, 
the  +2  and  +5  degree  intervals  are  tested  to  make  sure  that  the  mid¬ 
point  correlation  C(ei)  was  greater  than  or  equal  to  the  end  point 
correlations  C(®2 )  and  C(© 3 ) •  If  either  is  not,  the  interval  is  shifted 
by  2  or  5  degrees  in  the  direction  of  the  larger  correlation  and  the 
mid-point  is  tested  again.  This  dual  pass  computation  reduces  the 
unwanted  coupling  effect  of  the  attitude  variables.  The  +5  degree 
calculation  detects  the  general  angle  of  maximum  correlation.  The 
+2  degree,  calculation  reduces  the  estimated  angle  error  by  not  being 
as  sensitive  to  unsymmetry  and  nonlinearities  in  the  correlation  func¬ 
tion. 


A  sample  of  the  dual  pass  azimuth  estimation  is  illustrated  in 
Fig.  11.  For  this  sample,  noisy  edge  data  of  a  simulated  aircraft  at 
an  azimuth  angle  of  -45.0  degrees  is  used  as  input  data.  This  data 
is  correlated  with  computer-generated  edge  data  from  a  stored  air¬ 
craft  model.  The  initial  estimate  of  attitude  is  -48.0  degrees.  The 
final  azimuth  calculated  is  -44.11  degrees,  a  0.89  degree  error  from 
the  true  -45.0  degree  aircraft  azimuth  attitude.  This  is  a  typical 
maximum  error.  For  example,  if  the  initial  estimate  of  azimuth  is 
-42.0  degrees,  the  final  error  would  be  0.0  degrees. 
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Figure  11 

Azimuth  Correlation  Function 
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This  section  has  introduced  the  use  of  two-dimensional  correla¬ 
tion  to  determine  the  attitude  of  a  three-dimensional  object.  Equation 
(24)  is  used  as  a  second  order  polynomial  model  for  the  correlation 
function  of  each  attitude  variable.  This  approximation  reduces  the 
number  of  computations  necessary  to  determine  the  peak  correlation  for 
each  attitude  variable.  This  quadratic  approximation  was  demonstrated 
with  a  computer  simulation  that  tested  for  the  maximum  correlation  of 
an  image  as  a  function  of  attitude.  The  simulated  image  was  previously 
generated  by  adding  edge  noise  to  a  computer-generated  image  at  a  known 
attitude  of  -45.0  degrees.  By  starting  the  correlation  algorithm  with 
a  -3.0  degree  offset,  the  final  error  was  found  to  be  0.89  degrees. 
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V.  MATCHED  FILTER  EQUATION 

It  is  well  known  that  one-dimension  correlation  can  be  calculated 
by  a  simple  multiplication  of  two  functions  that  are  Fourier  trans¬ 
formed  [9]. 

c(t)  *  x  (t)  h(t+i)  dx  =  x(t)  *  h(-t)  (28) 

where  x  is  the  dummy  variable  of  integration  and  *  is  used  to 
symbolize  the  convolution  integral. 

C(w)  =  X(w)  H*(w)  =  X*(w)  H(w)  (29) 

where  w  =  2*f  =  frequency  in  radians  and  *  is  used  to  indicate  com 
plex  conjugation;  that  is,  if 

H(w)  =  R(w)  +  jl(w),  then  H*(w)  =  R(w)  -  jl(w). 

c(t)  <=>  C(w)  (30) 

where  <=>  is  used  to  indicate  a  Fourier  transform  pair;  that  is 

C(w)  is  the  Fourier  transform  of  c(t). 
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This  same  relationship  can  be  shown  in  two-dimensions  and  in  discrete 
form  [5]. 


N-l  M- 
c(k,1 )  =  E  Z 
i=0  j=i 

C(m,n) 

C(m,n)  = 


x  (i . j }  h(k+i,l+j) 


(31) 


X*(m,n)  H(m,n) 


(32) 


K-l  L-l 

j  2  nmk  ,•  2  n  nl 
z  z  x(k,l)eJ  K  e  J  L 
k=0  1=0 


1-1  0-1 

.  2  n  mi 

z  1  h(i  ,j  )e  J  I 
1=0  j=0 


(33) 


m  =  variable  in  first  dimension 


n  =  variable  in  second  dimension 


K  =  I  =  M  =  Period  in  first  dimension 


L  =  J  =  N  =  Period  in  second  dimension 
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A  complete  listing  of  the  transformation  indicated  in  equations  (32) 
and  (33)  is  given  in  Appendix  A. 

The  reason  the  Fourier  domain  correlation  approach  is  used  is 
that  for  large  data  arrays  the  Fast  Fourier  Transform  (FFT)  computa¬ 
tion  time  is  less  than  that  of  direct  time  or  space  domain  correlation. 
Also,  Fourier  transform  and  array  processing  hardware  is  available  for 
many  scientific  computers. 

The  continuous  correlation  equation  (28)  states  that  an  infinite 
integration  is  necessary  to  determine  the  correlation  at  a  particular 
time.  This  is  not  the  case  for  finite  sampled  functions.  The  summa¬ 
tion  interval  for  the  sampled  functions  requires  that  the  majority  of 
the  information  in  the  two  functions  be  contained  in  the  finite  sampled 
interval  for  the  correlation  to  approximate  the  continuous  correlation. 
When  x(t)  and  h(t)  are  sampled  with  an  impulse  function  A(t)  over  a 
finite  interval,  0<t<t  =  NT,  their  Fourier  transforms  x(w)*a(w) 

and  H(w)*a(w)  will  be  periodic  with  period  w  =  1/T,  where  T  =  sampling 
interval  of  the  impulse  function  A(t).  When  the  continuous  function 
x(t)  is  sampled  over  the  interval  0  <  t  <tmav  and  Fourier  transformed, 
this  is  equivalent  to  calculating  the  Fourier  Series  coefficients  of  a 
sampled  periodic  sequence  xp(nT)  of  period  N,  where  N  =  tp/T  and 
T  =  time  interval  between  samples.  If  xp(nT)  is  time  shifted  in 
relation  to  x(nT)  by  m  samples,  and  the  first  N  samples  are  trans¬ 
formed,  it  can  be  shown  that  the  resulting  complex  Fourier  transform 


will  change  in  phase  but  not  in  amplitude  or  period  [9].  In  the 
time  domain,  this  shift  of  m  samples  is  due  to  the  periodicity 
of  Xp  (nT)  and  is  referred  to  as  a  circular  shift  of  a  sequence  [5]. 

The  sequence  appears  to  be  circular  because  the  last  sample  of  x(nT) 
that  is  shifted  beyond  the  interval  N  T  appears  at  the  beginning  of 
the  sequence.  Likewise,  in  the  two-dimensional  correlation  there  is  a 
circular  shift  in  two  dimensions  when  one  image  x(i,j)  is  correlated 
with  another  h(i,j).  The  two-dimensional  periodic  or  circular  sampled 
function  is  equivalent  to  joining  parallel  horizontal  and  vertical  edges 
of  each  image  and  rotating  one  image  on  each  of  its  axes  while  the 
other  image  remains  stationary.  This  two-dimensional  circular  shifting 
is  depicted  in  Fig.  12.  In  this  figure,  image  2  has  shifted  n  samples 
in  the  horizontal  direction  with  respect  to  the  fixed  image  1.  Image  2 
has  shifted  m  samples  in  the  vertical  direction  with  respect  to  the 
fixed  image  1.  To  clarify  that  image  2  has  circularly  "wrapped  around" 
image  1,  the  corners  are  labeled  A,  B,  C  and  D. 


The  continuous  images  1  and  2  are  sampled  at  N  and  M  discrete 
points  in  the  x  and  y  axis  respectively.  This  sampling  results  in 
a  periodic  equation  (33)  in  the  transformed  frequency  domain  of  per¬ 
iod  N  and  M.  If  the  correlation  product  of  the  two  sampled  images 
has  nonzero  terms  beyond  the  N  and  M  dimensions,  there  will  be  an 
overlap  of  nonzero  samples  in  the  frequency  domain.  This  overlap 
phenomenon  of  a  high  frequency  component  taking  on  the  identity  of 
a  lower  frequency  component  is  referred  to  as  frequency  aliasing. 
Likewise,  spatial  aliasing  would  occur  if  there  was  an  overlap  of  the 
periodic  spatial  functions;  for  example,  if  the  first  and  last  samples 
added. 

To  test  the  correlation  method  of  attitude  measurement,  a  com¬ 
puter  simulation  program,  FAC  listed  in  Appendix  B,  was  run.  Discrete 
boundary  points  were  generated  as  input  data  for  a  known  aircraft 
attitude  as  illustrated  in  Fig.  13.  A  noisy  test  object  was  generated 
that  was  within  +5  degrees  in  azimuth,  elevation  and  roll  of  the  ref¬ 
erence  object.  The  noisy  boundary,  as  described  in  the  next  paragraph, 
was  compared  with  the  noise-free  computer-generated  edge  data  to  esti¬ 
mate  the  aircraft  attitude.  The  representative  errors  of  attitude 
(azimuth,  elevation  and  roll)  were  computed  by  comparing  the  attitude 
that  was  iteratively  predicted  by  equation  (27)  with  the  actual  atti¬ 
tude.  These  results  are  summarized  in  Table  II. 
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Figure  13 

Boundary  Points  of  FACIT  Model 
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Table  II. 


Representative  Attitude  Errors  (Degrees) 

Three  Sample  Average,  1  Pixel  Noise  Standard  Deviation 


Azimuth 

0.12 

Elevation 

0.55 

Roll 

0.71 

It  was  noted  that  the  errors  increased  with  each  attitude  measurement. 
This  is  because  of  the  sequential  coupling  of  a  previous  attitude 
angle  error  to  the  next  quadratic  angle  interpolation. 


The  quadratic  correlation  attitude  estimator  (27)  was  initial¬ 
ized  with  data  to  within  10  degrees  of  the  known  aircraft  attitude. 
Boundary  points  on  a  32  x  32  grid  were  generated  from  the  two- 
dimensional  projection  of  the  test  aircraft.  The  number  of  boundary 
points  generated  were  a  function  of  the  test  aircraft  attitude,  but 
typically  108  points  were  generated: 


(27  each:  maximum  horizontal,  minimum  horizontal,  maximum 
vertical,  minimum  vertical). 
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To  simulate  detector  noise,  Gaussian  noise  with  zero  mean  and  one 
pixel  standard  deviation  was  added  to  each  boundary  point,  horizon¬ 
tally  and  vertically.  Since  the  boundary  point  function  was  on  a 
discrete  32  x  32  grid,  this  had  the  effect  of  quantizing  the  noise 
to  unit  pixel  intervals.  For  example,  if  the  absolute  value  of  the 
Gaussian  noise  was  less  than  0.5  pixel,  no  noise  would  be  added.  If 
the  absolute  value  of  the  Gaussian  noise  was  between  0.5  and  1.5 
pixels,  one  pixel  of  noise  was  added.  Likewise,  two  or  more  pixels 
of  noise  would  be  added  for  greater  outputs  of  the  generator.  These 
noisy  boundary  points  are  shown  in  Fig.  14. 

An  uncalibrated  system  test  with  real  data  was  made  from  a 
digitized  video  image  of  a  FI 02  aircraft.  The  intensity  data  from 
the  image  was  transformed  into  edge  points.  The  edge  points  were 
correlated  with  the  model  data  stored  in  the  computer  memory.  This 
predicted  the  aircraft's  attitude. 

The  x,y  resolution  of  the  digitization  was  340  pixel  columns 
x  220  pixel  rows  and  the  intensity  resolution  was  1  to  256.  As  the 
photograph  in  Fig.  15  shows,  the  aircraft  image  makes  up  only  a  small 
portion  of  the  video  image.  Therefore,  a  64  x  64  pixel  window  shown 
in  Fig.  16  was  contrast  enhanced,  in  that  the  original  intensity 
range  of  103  to  156  was  linearly  transformed  to  a  1  to  256  intensity 
level.  It  is  noted  that  the  digitized  image  is  not  displayed  at  the 
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Figure  15 

Photograph  of  FI 02  Aircraft 


correct  aspect  ratio  due  to  the  unequal  sampling  ratios  taken  verti¬ 
cally  and  horizontally. 

The  edge  data  from  Rosenfeld's  Detector  [7]  was  applied  to  a 
variable  threshold  iteration  that  was  adjusted  until  greater  than 
200  pixels  were  above  the  threshold.  This  adjustment  was  necessary 
so  that  low  contrast  edge  pixels  typically  at  wing,  nose  and  tail 
extremities  would  exceed  the  threshold.  These  extremities  are  most 
important  in  obtaining  a  given  attitude  accuracy.  Fig.  17  shows  the 
results  of  the  best  computer-generated  match  of  the  stored  image 
superimposed  on  the  edge  data  obtained  from  the  video  image.  The 
computer  predicted  the  attitude  to  be: 

Azimuth  =  -115.0  degrees 
Elevation  =  45.0  degrees 

Roll  =  -  5.0  degrees 

There  was  no  other  data  available  to  establish  the  attitude  of  the 
F102  aircraft  at  the  instant  the  camera  recorded  the  image.  It  is 
presumed  that  further  investigation  would  show  this  attitude  to  be 
correct  to  within  the  limits  of  the  image  resolution  and  the  edge 
noise  present. 
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Figure  17 

Best  Fit  of  Stored  Image  Pixels  to  Video  Outline  Pixels  of  Fig.  16 
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The  lack  of  sharpness  in  the  transformed  edge  data  shown  In 
Fig.  17  can  be  attributed  to  several  factors; 

1.  Low  x,y  resolution  (64  x  64). 

2.  Low  intensity  range  (103  -  156). 

3.  Uneven  lighting  and  shading  on  edges. 

4.  A  simple  edge  detector. 

The  last  factor  would  be  the  only  one  that  could  be  significantly 
changed  given  a  free-flying  aircraft,  fixed  camera  locations,  and 
natural  lighting.  A  review  of  methods  used  for  extracting  features 
based  on  edges  and  contours  is  included  in  a  survey  by  Lavine  [11]. 
More  recent  theories  and  experiments  on  edge  detectors  have  been 
demonstrated  by  Abdou  and  Pratt  [12]. 

This  section  has  introduced  the  use  of  two-dimensional  Fourier 
domain  correlation  as  described  by  equation  (32).  Representative 
results  of  three  computer  correlation  simulations  were  listed.  The 
simulation  compared  a  computer  generated  edge  model  to  a  previous 
computer  generated  model  with  added  edge  noise.  Finally,  a  sample 
correlation  of  the  computer  generated  edge  model  with  the  edge  data 
detected  from  a  real  video  image  of  an  aircraft  at  an  unknown  atti¬ 
tude  was  described. 


45 


VI.  AIDED  AIRCRAFT  TRACKING 


An  example  of  the  utility  of  the  attitude  measurement  is  demon¬ 
strated  by  utilizing  an  aircraft  positional  estimating  Kalman  Filter 
with  combined  radar  and  electro-optic  sensor  measurements  [10].  It 
is  difficult  for  the  radar-only  estimator  to  keep  a  close  target  in 
track  if  it  is  highly  maneuverable.  The  criterion  of  "highly  man¬ 
euverable"  for  a  particular  radar  is  dependent  on  the  following: 

1.  The  sampling  interval  of  the  radar. 

2.  The  range  gate  period,  or  the  sampling  aperture. 

3.  The  receiver  sensitivity. 

4.  The  return  signal  to  noise  ratio. 

5.  The  mount  azimuth  and  elevation  dynamics,  or  time  constants. 

6.  The  measurement  accuracy  of  azimuth,  elevation  and  range. 

7.  The  point  source  tracking  of  a  finite  length  target. 

These  parameters  and  others  contribute  to  the  ability  of  the  radar 
to  track  with  minimum  error.  By  modeling  the  aircraft  target  to  be 
driven  by  an  acceleration  function  that  is  dependent  upon  aircraft 
attitude,  more  accurate  range,  range  rate  and  range  acceleration  state 
estimates  are  possible.  The  acceleration  function  is  based  on  the 
well-known  aerodynamic  coupling  between  the  target  attitude  and  the 
direction  of  the  target  acceleration.  This  coupling  is  summarized 
in  the  following  comments: 


46 


1.  Major  accelerations  (lift)  are  normal  to  the  velocity  vector 

and  the  pitch  axis. 

2.  Positive  lift  is  more  likely  than  negative  lift  due  to  pilot 

physiological  factors  and  to  structural  loading  design. 

3.  Accelerations  in  the  velocity  direction  (drag/thrust)  are 

generally  smaller  in  magnitude  and  of  shorter  duration  than 

the  lift  accelerations. 

A  complete  multiple  measurement  system  using  the  previously  discussed 
imaging  attitude  measurement  concept  as  a  radar  tracking  aid  to  the 
positional  estimating  Kalman  Filter  is  shown  in  Fig.  18.  In  the  upper 
left  corner  of  Fig.  18  there  are  n  radars  that  provide  range,  azimuth 
and  elevation  measurements  to  the  Iterated  Extended  Kalman  target 
estimator.  In  the  lower  left  corner  there  are  m  video  cameras  that 
input  data  to  m  correlators.  These  correlators  first  scale  the  stored 
model  to  a  size  equal  to  the  predicted  input  image  size.  The  scaling 
is  a  function  of  target  range  and  system  gains  such  as  telescope  mag¬ 
nification  and  image  pixel  sampling  area.  The  correlators  then  apply 
equation  (33)  as  described  in  the  previous  section  for  the  computer 
simulation  FAC.  The  attitude  measurements  azimuth,  elevation,  and 
roll,  from  the  correlators,  are  transformed  into  a  common  coordinate 
system.  The  predicted  attitude  of  a  model  aircraft  in  a  coordinated 
turn  is  combined  with  the  m  attitude  measurements  in  a  Kalman  Filter. 


Radar  1  I  „  I  Target 


Figure  18.  Attitude  Measurement  and  Tracking  System 
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The  estimated  attitude  from  the  Kalman  Filter  predicts  an  accelera¬ 
tion  vector  that  is  used  as  an  input  to  the  target  estimator. 

The  simulation  of  a  target  range  (R)  estimation  was  performed 
with  the  program  KAL  listed  in  Appendix  B.  The  simulation  KAL  was  a 
limited  example  of  the  radar  tracking  filter  as  illustrated  in  Fig.  18. 
This  simplification  was  made  possible  by  not  including  some  tracking 
variables,  such  as  azimuth  and  elevation,  to  the  full  three-dimensional 
trajectory  filter.  The  trajectory  was  truly  three-dimensional,  hence 
there  were  changes  in  azimuth  and  elevation,  but  they  were  not  used 
as  system  measurements  or  estimated  as  parameters. 

The  Iterated  Extended  Kalman  Filtering  subroutine  FT KAL I  is  a 
modification  of  the  IMSL  Kalman  Filtering  subroutine  FKALM  [14]. 

The  system  was  modeled  as  a  radar  track  of  a  nominal  constant  accel¬ 
eration  target.  The  assumptions  for  this  trajectory  are: 

1.  The  average  acceleration  rate  is  zero. 

2.  The  acceleration  rate  between  radar  scans  is  constant. 

3.  The  acceleration  between  scanning  intervals  in  uncorrelated. 

The  above  assumptions  are  described  in  the  well-known  Alpha-Beta- 
Gamma  Filter  equations  [13].  Written  in  a  discrete  form,  these 
equations  are: 
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(34) 


x  =  x„  T  +  T  x  ,  +  -5 —  ar  , 
n  n- 1  n-1  2  n-1 


(35) 


x'  =  V  ,  +  T  ar  , 
n  n-l  n-1 


(36) 


where  ar  is  a  random  sequence  with  zero-mean  and  a  mean-square 
acceleration  rate  of 


a2  for  j  =  k 
0  otherwise^ 

The  target  trajectory  was  simulated  in  three  parts: 


E[arj  ark]  = 


(37) 


1.  A  target  constant  closing  velocity  of  100  or  175 
yards/second  at  the  same  altitude  as  the  radar. 

2.  A  10-g  target  turn  in  the  azimuth  plane  of  the  radar. 

3.  A  10-g  target  turn  "up"  in  the  geometric  plane  perpendicular 
to  the  azimuth  plane  and  the  ground  projection  of  the  radar 
range  vector. 
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The  final,  or  third  turn,  starts  when  the  target  has  rotated  through 
90.0  degrees  of  the  10-g  turn  in  the  azimuth  plane.  The  maneuver 
was  started  at  three  initial  ranges: 

1.  Distant  :  11275.0  yards 

2.  Mid-range  :  1000.0  yards 

3.  Close-in  :  150.0  yards 

Inputs  to  the  filter  of  two  noisy  radar  range  measurements  and  an 
optional  radial  component  of  the  predicted  normal  acceleration  vector 
were  generated  at  a  sample  rate  of  10/second.  A  one  sigma  standard 
deviation  Gaussian  noise  of  1.0  yard  was  added  to  the  two  range 
measurements.  These  inputs  were  input  to  the  range  filter. 

Typical  results  of  range  error  for  one  simulation  of  all  man¬ 
euvers  are  listed  in  Table  III.  The  addition  of  the  predicted  normal 
acceleration  measurement  to  the  radar  range  measurements  reduced  the 
peak  and  RMS  errors  for  all  maneuvers.  The  reduction  in  error  was 
most  dramatic  for  the  close-in  maneuver  because  of  the  range  reversal 
for  the  particular  azimuth  and  elevation  geometries.  At  the  mid¬ 
range  and  distant  range  trajectories  there  was  no  range  reversal 
so  the  additional  acceleration  measurement  was  less  helpful  in  cor¬ 
recting  the  range  and  range  rate  estimates  In  addition,  the  high 
radar  sample  rates  and  the  averaging  effect  of  using  two  radar  measure¬ 
ments  reduced  the  difference  in  error  between  the  radar-only  estimate 
and  the  combined  estimate. 
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Table  III. 


Range  Error  (RMS  Average/Peak  -  in  Yards) 


Maneuver 

Combined  Measurement 

(Normal  Acceleration 

and  Two  Radars) 

Two  Radars  Only 

1.  Distant 

0.76/3.22 

0.99/4.82 

2.  Mid-Range 

0.89/4.25 

1.06/5.53 

3.  Close-In 

4.83/7.56 

10.28/12.02 

Fig.  19  shows  the  aircraft  10-g  turn  trajectory  plotted  in  three 
dimensions  for  a  175  yards/second  air  speed.  As  compared  to  the  100 
yards/second  air  speed  trajectory  shown  in  Fig.  20,  the  distance 
traveled  down  range  is  much  greater,  and  the  turning  radius  was  much 
larger.  However,  within  the  5  second  interval  of  flight,  the  air¬ 
craft  in  Fig.  19  did  not  climb  as  high  as  that  in  Fig.  20.  This  is 
because  the  third  maneuver  (turn  up)  did  not  start  until  approximately 
3  seconds  versus  2  seconds  for  the  100  yards/second  trajectory. 
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E\_EVM\OU  -  VDS. 


TARGET  TRAJECTORY 


LEGEND 

□  =  3-D  PLOT 
o  =  GROUND  PLOT 


Figure  19 

Three  Dimensional  Aircraft  Trajectory, 
Velocity  =  175  yards/second 
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rr 


TARGET  TRAJECTORY 


LEGEND 
□  =  3-D  PLOT 
o  =  GROUND  PLOT 
a  =  ELEV.-OFF  RANGE 


Figure  20 

Three  Dimensional  Trajectory, 
Velocity  =  100  yards/second 
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In  Fig.  21  the  radial  error  and  the  measurement  noise  in  yards 
is  plotted  versus  time  for  the  close-in  maneuver.  The  radial  error 
is  equal  to  the  true  radial  distance  minus  the  estimated  radial  dis¬ 
tance  in  yards.  Gaussian  noise  with  zero  mean  and  a  standard  deviation 
proportional  to  the  square  root  of  the  estimated  radial  range  variance 
was  the  measurement  noise  added  to  the  true  range.  For  this  example 
the  noise  has  a  1  sigma  standard  deviation  of  2.0  yards.  The  very 
large  peak  error  at  0.8  seconds  is  due  to  the  aircraft  flying  very 
nearly  "overhead".  This  then  reverses  the  sign  of  the  relative 
velocity  vector. 

The  velocity  reversal  is  shown  in  Fig.  22  where  X(2)  is  the  Kalman 
estimator  velocity  state,  and  VR2D  is  the  first  difference  of  range 
divided  by  the  sampling  interval.  The  slow  time  constant  of  X(2)  is 
due  to  the  lack  of  a  velocity  measurement.  A  more  accurate  velocity 
estimate  is  shown  in  Fig.  23  for  the  distant  turn  maneuver.  There  is 
no  velocity  reversal  in  this  trajectory. 

Fig.  24  shows  the  range  acceleration  state  estimate,  X(3),  and 
the  measured  range  acceleration,  Y(3).  The  two  accelerations  track 
quite  closely  and  describe  two  phenomenon: 

1 .  An  initialization. 
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Figure  21 

Range  Error  and  Measurement  Noise 
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TIME  IN  SEC. 


Figure  22 

Estimated  Range  Velocity  and  Differential  Range  Divided 
by  the  Sampling  Interval,  Close-In  Maneuver 
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TIME  IN  SEC. 


Figure  23 

Estimated  Range  Veil  city  and  Differential  Range 
Divided  by  the  Sampling  Interval,  Distant  Maneuver 
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1 


LEGEND 


Figure  24 

Range  Acceleration  Estimate  and  Range  Acceleration  Measurement 
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2.  Cyclic  acceleration  matching  the  period  of  the  third 
part  of  the  maneuver. 

The  acceleration  state  estimate  in  Fig.  25  with  no  acceleration 
measurement  does  not  show  this  periodic  acceleration,  indicating  the 
uncertainty  present  in  the  state  estimate. 

This  section  applied  the  correlation  attitude  measurement  tech¬ 
nique  to  the  radar  tracking  problem.  The  attitude  is  related  to 
acceleration  and  therefore  a  useful  measurement  in  predicting  target 
acceleration.  Recursive  trajectory  equations  were  used  in  a  computer 
simulation  that  estimated  aircraft  range.  The  simulation  demonstrated 
that  the  Kalman  Filter  was  more  accurate  in  tracking  a  target  with 
varying  trajectories  when  the  predicted  acceleration  was  used  as  in 
addition  to  the  noisy  range  measurements. 
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Figure  25 

Range  Acceleration  Estimate  Without  Range  Acceleration  Measurement 
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VII.  CONCLUSIONS 


Manual  optical  image  comparisons  have  been  the  primary  method 
of  measuring  attitude  of  three-dimensional  objects.  Section  V  lists 
the  general  two-dimensional  Fourier  transform  equations  that  are 
applied  to  an  images  edge  data  and  a  stored  model  of  the  solid  ob¬ 
ject.  The  attitude  of  a  three-dimensional  object  is  estimated  by 
using  a  three-axis  quadratic  interpolation  to  sequentially  maximize 
the  correlation  output  as  a  function  of  the  attitude,  position  and 
size  variables.  Moreover,  this  measurement  ur  estimate  is  equivalent 
to  the  maximum  output  of  a  bank  of  matched  filters. 

This  concept  was  then  applied  to  realistic  Kalman  Filter  track¬ 
ing  simulations  and  shown  to  give  improvement  over  conventional  target 
state  estimators  that  do  not  use  the  attitude  measurement  as  a  pre¬ 
dictor  of  target  acceleration. 

Since  the  necessary  attitude  measurement  is  within  the  electro- 
optical  tracker  and  computer  state-of-the-art,  the  use  of  this  esti¬ 
mator  concept  can  improve  present  tracker  system  capabilities. 
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APPENDIX  A 

TWO-DIMENSIONAL  DISCRETE  CORRELATION 


This  appendix  demonstrates  that  the  two-dimensional  discrete 
correlation  transform  (31),  (32)  is  equivalent  in  both  the  time  domain 
and  the  frequency  domain. 

N-l  N-l 

c(k,l )  =  2  z  x(i ,j )  h(k  +  1 ,  1  +j )  (31) 

i=0  j=0 


C(m,n)  =  X*(m,n)  H(m,n)  (32) 

where  *  =  complex  conjugate 

Equations  (31)  and  (32)  are  verified  by  performing  the  Fourier  Transform 
and  conjugation  operations  indicated. 


M-l  N-l 
c(k,l )  =  z  z 
i=0  j=0 


l 


M  N  m=0  n=0 


.  2n  .  .  2n 

-J  -fr1m  “J  — n“  J  n 
X*(m,n)  e  M  e  N 


P-1  Q-l 


2n  2n 

j  - p(k+i)  j  - q  (1+j) 

1  Z  Z  H(p,q)  e  M  N 

PQ  p=0  q=0 


(A-l) 


M-l  N-l  P-1  Q-l 


2n  2n 

1  . .  ‘  j  -n — P  k  j  -si — q  1 

c(k,l)  =  -m—  E  z  X*(m.n)  H(p,q)  e  e 

m=0  n=0  p=0  q=0 


(A-2) 


2n  2 n  2n  2 

N-l  N-l  -j  1  m  j  j  n  j  -j^-p  i  j  -|^-q  j 
1  z  z  e  e  e  e 

L  “RfT  1=0  j=0 
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The  term  in  brackets  in  equation  (A-2)  is  equal  to; 


-jjjjjj-  (M  N)  =  1 ,  if  m  =  p  and  n  =  q 


0  ,  if  m  t  p  and  n  ^  q 

due  to  orthogonality  of  i  and  j.  Therefore: 


1  M'1  N_1  1  —  m  v  i  —  n  1 

c(k>1)=~Hir  s  z  x*(m,n)  H(m,n)  e  3  „  mkeJ  N  n1 
m  m=0  n=0  n  N 

(A-3) 

=  2  1  fx*(m,n)  H(m,n)l 
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APPENDIX  B 


COMPUTER  PROGRAMS 

This  Appendix  is  composed  of  the  listings  of  the  Fortran  programs 
and  subroutines  for  the  attitude  measurement  and  target  tracking 
simulations.  The  programs  were  run  on  a  Univac  1110  computer  with 
a  Fortran  compiler  and  a  FLECS  translator  (Fortran  Language  with 
Extended  Control  Structures).  The  graphical  outputs  were  plotted 
using  DISSPLA  programs  that  generated  C0M-80  crt  images. 
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u  1  •'*  c  N  b  1  L‘  N  wH(1c»5oh)  •»hu(1o3-<*) 

D  1 1  t  N  S  I  ON  S  (  1  2o  )  .  1  u  V  (  1  <.  )  .  M  ( l  ) 

U  I  .•»  t  N  S  I  L  N  4ZAf&',eLA(6),KULLA<C',Cl"**(c"> 

DIMENSION  Z<C~u> 

COMPLEX *o  A(16io4) 

COM  hh(t.WH,A.Crt-X.i 

DO  111  J  =  1 , 2  u 

CfAX(J>=-1.Pe10 
CONTINUE 
IPLOl=1 
I  f‘  K  l  M  =  1 

REAL  ( !>  .  1  e  )  Al.tl.K  CLL.LAiT 
FOkpaTO 

fc  n l T  t ( 0 . IjI )  AZ. EL. ROLL. LAST 
FOk  i-rtTT1X.3Fk.2.l4) 

IF(LaST.EU.I)  I  P  L  U  T  =  3 

read  ARRAY  SIZE  IN  POWERS  OF  c 
REAL  NUMBER  OF  ARRAYS  TO  btS  CORRELATED  *  M  A  A  OF  20' 


REA0(i.5)NK.Jj 
FORM  ATT  21 5) 
fifA0(5.5)  HA 
n1=2**nk 
N2=2**JJ 
n3  =  1 

kk2=kka 1 

J J2=JJ*1 

N12=2*N 1 

N22  =2  *N2 

N  H  =  N  1  /  2 

NJ=N2/2« . 5 

NV=N  1  •  N  < 

nV4=n12*nc2 

NC=  ( N 1  *N2  )  /  2  4  ♦  1 

Ml  )=KK2 

m  *2  >  =  J J  2 

M  (  3  )  =  L- 

PI  =  3.1415V 

kRI  Tt (6 . 1  5)  M  ,N  2 

FORMAT*'  InPUT  aRRA*  Dl^ENVIONi 


*  ( ',1 i.'x'.I 


HEAD  IN  TEE  UAlA  ArRAY 


»  R  I  T  t  <  o  .  a  > 

DO  11  N  =  1  .NC 
K  Is  (A  “1  )  *  £4  ♦  1 
n  24  =a  *<:>. 

(lE  A  C  <  5 . 7)  T»H  <  I  )  ,  I  =K  1  .  K  t  A  ) 
»HlTt(C.V)<1.(«n(I).Is;K1.AE4) 
C  0  N  T  i  N  U  t 
f  OH  A  A  T  (  i  s  F  I  .  ’.  ) 


FOHiM('  I  '.2*l'»b(I  )')./>) 

9  FOR  r-  aT(  IX  .  1  5  .  24  (  IX  .  F  4 .0)  ) 
l 

C  OGUllE  SIZE  OF  DAT/.  ARRAY 

t  wp  =  input  vector.  »ki<  =  output  vector,  nv  =  input  dimension 

c 

CAUL  V  D  OU  BL (mH.mHR .nv  .  N  v  o 
c 

C  PLOT  DATA  APnAY 

t. 

c 

C  TRANSFER  THE  Vti  (K  )  DATA  TO  THE  COMPLEX  A  MATRIX 

C 

DO  2  12  =  1  .  N  2  E 
DO  3  11=1  .N12 
Iwh=  II2-1)*N1*IV 
A(IWH)  =  hHR( I m  H ) 

1  CONTINUE 

c 

C  COMPUTE  THE  DISCRETE  FOURIER  TRANSFORM  OF  THE  OH(K)  DATA 

C 

CALL  HARM (A .M , lNV.a  .1 .  IPeRR > 

C 

C  READ  IN  ThE  SECOND  DATA  ARRAY 

C 

C 

C  ROTATE  MODEL  IN  AlbU Th 
C 

NL  =  0 
N  A  1  =  1 
DELAL  =5. 

AZA<1)=AZ 
AZA (<)=AZ-DELAL 
AZAT3)=AZ«  DELAL 
290  CONTINUE 

DO  300  J  =  NA 1  ,N A 
C 

C  GENERATE  IMAGE  aOUN  D Ak  X  FROM  3 -D I M E N T I  ON AL  MOuEL 
C  «H  R  =  CaRD  =  OUTPUT  VtCTOR  (INCYM  X  INCYM) 

C 

CALL  SURFAC (AZA(J) ,EL«kOLL.YMAX.2MAX,YAlN, iMIN.J .IPLOT.IPRIN  T) 

C 

C  CALCULATE  CORRELATION  bETwEEN  INPUT  IM  A&  E  AND  GENERATED  IMAGE  30UND» 
C 

203  7  FOR MaT < / , 5x  ,  '  j  = ' , * 2 , 3 x  ,  'C M ax  =',F17.©,/) 

CALL  CALC0R(KK2.JJ»-.NV  .  AZ  A  <  J  >  .  EL  .  N  OLL  .  J  .  N  1  2  .  N2  2  > 

30o  CONTINUE 
NL  =  NL  ♦  1 

IF(nl.GT.o)  iiO  TO  s202 
WRI Te (6 ,310) 

i 1 L  FORMAT(/,5x.'J'.3».'AZA(J)'.7X.'CMAX<J)'./) 

DO  211  J  =  1 . 3 

kRI  Tc<Cp,312'  J  ,  AZ  A  <  J  )  ,  CM  AX  <  J  > 

311  CONTINUE 


I 


;1w  FCRi‘MT(HX«12.3*»Fo.Z.3x.F11.t) 

L 

L  Ttsr  fin  F  A  X  1  *  u  h  CORncLATIfN.  IF  C  I-  A  X  M  )  <  C  H  A  X  f  ?  )  OH  C  *  A  X  F  j  > 
L  CoF RELATE  AT  ADDITIONAL  PuIi.TS 

c 


IF(LrtAX(1  ).LT  .  C  »  A  a  i  £  )  ) 

6  0 

T  C. 

i  1  A  1 

1  F  (  U-  A  X  <  1  T  .  L  T  .  C  /  A  x  <  M  ' 

6  0 

TO 

7  1  A  2 

60  TO  iUA 

•  1  A  1  IF ( CMAX (2 ) .L T .CnAX l 2)  ) 

Go 

T  0 

i  1  A  2 

A  7  A ( i>  =  A2 AT  1  ) 

AZA ( 1>  =  A2  A(2) 

AZA  U)  =  A2  A(2)-DELAl 
C^*ax(3)=CNax  (1) 

CPA  X ( 1 ) =  CPAX  < 2) 

OA  X  <  c)  =-  1  .E  10 
N  A  1  -  L 
N  A  =  Z 

60  TO  2vG 
•1-.0  CONTINUE 

AZA  <  2 )  =  AZ  A<  1  ) 

AZA (1)=A2A<3) 

AZA I 3>=AZAf SULCAL 
CPA  X  <2>  =CF  AX (  1  ) 

C*AX(1)SC*'AX(3) 

CPA*(3)=-7.E10 
NAl  =  J 
N  A  =  2 

o  0  T  o  2  v  C 
i  1  CONTINUE 

L  CALCULATE  FlOST  PRObABLE  AZMUTH  FOR  INPUT  FhAHE  BV  oUDRATIC  INTERF 

C 

AZST=(CMAX(2)-Cf*AX(5))*DtLAL  /(2.*CPAX(I)+<:.*Cf1AX<Z>-4.*CMAX(1>> 
IAZiT  =  lFIX<A2SI*.5)*lFIX<AZAMM 
222t  F0RFATIZA6) 

PR  I  NT  32G.AZST, lAZiT 

32o  FOR  F  ATT  /  ,  5X  ,  '  AZ  S  T  *  '  ,  F  1  2 . 5  ,  5x  ,  '  I  A  L  S  T  ='.IA,/> 

C 

t  HtOUCE  INTERVAL  OF  INTERPOLATION  if  AdS.  VALUc  of  AZST  >  1. 

c 

IF( AeSCAZST)  .LT.1.  J  bO  TO  7202 
DEL  AL-2  .U 
N  A  1  -c 
N  A=  3 

AZA  <2)  =  AZ  A ( 1 ) -u  E  L  A  l 
AZA  <i)  =  AZ  AM  >*DELAl 
CPAX(2)=-1.E10 
C*Ax( 3)=-1.ElO 
GO  TO  2  9u 
I.  'c  CON  TINUt 

A  Z  =  FLOAT! IAZST) 
v 

^  rotate  ;<ioi.EL  in  elevation 

c 


Afi 


licLtPS  =  5. 

M*:.  ’• 

CMAX<7)=CF'AX  <  1  ) 

N  AS  =<5 
NA9  =y 
ELA ( 1 )=EL 
ELA  <2)=£L-0ELEpJ 
ELA  (3)=EL*DELEPS 
i9u  CONTINUE 

DO  AuO  J=NAo,NAV 
J  R=  j -e 

GtNERATc  IMAGE  cOUNOAnT  FnOM  3 -D  I  rl  E  h  T I  ON  A  l  MOi/EL 
Whfi  =  CaRU  =  OUTPUT  WtCTO R  (INCTH  X  InCT*) 

CALL  SUkFmC  (  A  2  »  c  L  A  (  J  M  )  .ROLL  .  Y  M  A  X  ,  ZM  A  X  ,  Y  M  1  N  . ZM I N  .  J  •  I  PL  0  T  « I  P  RI  NT  ) 

CALCULATE  CORRELATION  bETwEEN  INPUT  IMAGE  AND  GENERATcU  IMAoE  3  OUND  A 

CALL  CALCOR  <KK2  ,J  J  J  ,NV  .  A  Z  .  EL  A  (  J  H  )  .  ROLL  .  J  .  N  1  2  .  N  2  2 ) 

«*GC  CONTINUE 
NL=  NL ♦  1 

IF(NL.GT.O)  GO  TO  *.202 
MR  I Tt(6.410) 

A  1  0  FORMATt/.SX.'j'.SX.'ELAUMJ'.ZX.'trtAxCj)'./) 

DO  All  J*7. 9 
JM  = J-C 

GRI Tt (6 .312)  J . tL A  I  4M  )  .  CMAX ( J  > 

All  CONTINUE 

TEST  FOR  MAXIMUM  CORRELATION.  IF  CMAX(7)  <  CNAX(8>  OR  CMAx(9) 
CORRELATE  AT  ADDITIONAL  POINTS 

IFTCMAXm.LT.CMAX<8>>  GO  TO  A1C1 
IF < CMAX <7 ) .L T .CMAX < V) >  GO  TO  A  1  A  2 
GO  TO  A  1  A  A 

A  1  A  1  I  F < CMAX (6) .L T .CMAX < 9) )  GO  TO  aTA2 
ELA  <3>  =  EL All) 

ELA  <1)s:ELA<2) 

ELA (2)SELA(2 )-D£LEPS 
CMAX<9)=CMAX 17) 

CMAX(7)=CMAX (b) 

CMAX(b)*“1.E10 
NAb  =  0 
NA9=e 
60  TO  390 
AU2  CONTINUE 

ELA (2)  =  ELA(1  ) 

ELA<1)=ELA<3) 

ELA (3)*ELA(3)4wELEPS 
C  *  A  X  <b) =CMAX  ( 7) 

C  M  A X(7)  -  C  M  A  X  (<i) 

C*AX<9)=-1.E1C 
n  a  y  =  9 


0 


J—. 


6' 


N  A  3  =  9 
bO  T  u  3  VG 
-H*  CONTINUE 
C 

v  CALCULATE  MOST  PROcAbLt  ELEVATION  FOR  INPUT  FRAME  e»  OUDRATIC  INT 

C 

£LST  =  <CMAXTii>-CMAX<9>>*C'£LEPS/f2.*CNAX<9)*2.*CMAXfe'-4.*CMAxn>) 
IELST  =  IFIX(ELST>.5)-HFI*(ELA(1)) 
print  3  2 1  .  £  L  S  T  .  I  E  L  i  T 

321  F0RFAT(/.5X,'6LST  ='.F12.5.5x  .  ' I  £  LS  T  s'. 1 4,./) 

C 

C  REDUCE  INTERVAL  OF  I N 1 1 R P OL A T I  ON  IF  AbS.  wALUt  OF  ELST  >  1. 

C 

I F ( AdSC ELST )  .L T  .1 ,u )  bO  TO  4202 
DEL  EPS  =  2. 

NAS  =d  , 

N  A  9  s  V 

ELA ( 2 )  =  EL  A ( 1 ) -b  E  L  E  P  S 
EL  A  (j)»EL  AM  )  ♦DELEPS 
C  M  A  X (  d)=-1.£l& 

C  M  A  A  (  9)  =  -1.ElO 
bO  TO  390 
VtOt  CONTINUE 

EL=FLOAT(IELST) 

c 

C  ROTATE  MODEL  IN  ROLL 

C 

DEL  RH0  =  5  . 

NL=U 

CMAX(13)=CMAX<7> 

ROLLA(1)=ROLL 
ROLLA<2)=ROLL-DELRhO 
ROLLA(3)=ROLL*D2LRPO 
NA1 As  U 
NA1 5=15 
49C  CONTINUE 

00  SCO  J=NA14,NA15 
J  *-=  J  - 1  2 
C 

C  GENERATE  IMAGE  bOUNDARY  F*OM  3 -D I  ME N TI ON AL  MOuEL 
C  WhR  =  CARO  =  OUTPUT  VcCTOH  (INCTM  X  INCYM) 

C 

CALL  SUKFAC(A7tEL.HCLL«UM).YMAX.£MAX.rMIN.2MIN.J,IPLOT.IPKlNT) 

C 

C  CALCULATE  CORRELATION  b  E  T ■ E  £  N  INPUT  IMAGE  AND  GENERATED  INA«E  3OUN0A 
C 

CALL  CALCOR  (  AK2  .  J  J  ,NV  .  A£  .  EL  .hOLLA  <JM)  ,J  ,  h12.N22> 

50U  CONTINUE 
NL  s  NL ♦  1 

IF(NL.bT.fc)  bO  TO  5202 
wRI Tt  <  6 . 5  TO) 

510  FORMATt/. 5x • '  J  ' . 3  A • 'ROLLA(JK) '.7X  ,'CMAX  (J)  './) 

DO  511  J  *  1  3  .  1  5 
JM=  J-12 


4  R  I  T(_(o,312)  J  .ROLt.  A(Jfc)  .CPAX  (J  ) 

-  1  1  CONTINUE 

t. 

L  T  t  S  T  FC'K  MAXIMUM  CuRRlLATIOn.  IF  C  P  A  X  (  13)  <  C  l>  A  X  (  1  4  >  Ok  C  h  A  x  (1 5  ) 

L  CuRRELmTE  AT  ADuI  TIUNmL  points 

L 

I  F  (  CHAX  (  1  i )  .L  T  .CHA/  (  1  4  >  )  GO  TO  5141 
IF  (  IMA  X ( 1 3)  •  L  T  »  CM  A  a  (15))  GO  TO  5142 
GO  TO  5144 

>14  1  I  F  (  CmAX  M4>  .L  T.CMA/ (1  5  '  '  GO  TO  5142 
ROLLA  (  5  >  =  IOLL  A  C  1  ) 

H  OL  L  A  (  1  )=ROLLA(  t  ) 

H  OL  L  A  <  2  '=R0LlA<  1  >  -  U  E  L  (<  H  0 
CF*AXl15)»C''AX(13) 

CMX(13)sC**A<l4> 

Ci»AX(l4)  =  -1.Elu  i 
NAl 4=  14 
N  A 1  5=14 
GO  To  44U 
5 1 4  C  CONTINut 

ROL  L  A  <  2  '  =  ROLL  A  <  1  > 

HOLLA ( 1  )=KOLL A ( 3) 

RCLLA(3)=ROLLA(i)*u£LhPO 
C r*A  x  C 14  >  =  C«ia  X  <  1  i) 

C»AXM3'>C"A4n5) 

CMAX(15)*-1  •  e  1  U 
NAl 4=15 
NAl 5=15 
GO  TO  4-FC 
5144  CONTINUE 
C 

C  CALCULATE  MOST  PROd  AG  L  E  ROLL  FOR  INPUT  F  h AM  E  BT  wUDRaTIC  INTERP 

C 

ROLLST=(CPAX(l4)-CrtAX(15))*D6LRHO/<2.*CHAXl15)*2.*CKAX(U) 

*-4.  *  C  P  A  X  (  1)  ) 

IROLST=IFIX(RulLST*.5)«IFI*(ROLLA(  1 ) ) 

PRINT  323  . ROLLS!.  iROLST 

32  3  FOR  I*  AT(  /  .  5X.  *  ROLLS  T  =  '  .  F  1  2 . 5 , 5 X  .  '  IROLST  ='.I4,/) 

C 

C  REDUCE  INTERVAL  OF  IN  I ER POL  AT  I  ON  IF  AB  S  .  vALUc  OF  ROLLST  >  1. 

C 

I F ( AbS (ROLLS T ) ,LT . 1  .0)  GO  TO  52'12 
0ELRPG=2. 

NAl 4=  U 
NAl 5=15 

R0LLA(2)  =  RDLLA(1)-i;ELkH0 
RCLLA(3)  =  f.  OLLA(  I  )  ♦  i/ELni'O 
CMA  •  (  14  >  =  -1  .  c  1u 
Ci-A  X  l  1  5  )  =  -1  .  E  1  U 
G  C  I G  4  41. 

54 Uw  CONTINUE 
I  PL  0  I =  2 

IF  (LAST  .feta.1')  GO  T  \j  1 
STOP 
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S  l  n  }  A  l 


C 

C  SUn  F  AC  ROTATES  A  STLREO  i-0  INcimTIGNAL  MODEL  I(m  i  AXIS  A  2  >  J  T  n  .  fc  L  E  V  A  T- 
C  10/m  Iht  PROJECTED  IMAGE  Of  Tkfc  MODcL  ON  A  PLANt  SURFACE  IS  D  UA  N  T I  ZtD 
C  ONTO  A  (32X3 2)  LKIO.  OUTPuT  A  a  k  A  Y  =  'CAku' 

C 

S  LcR  GUT  1 NE  S  OkF AC  <  A2 . cL .ROLL  .YMAX 1 .Z  PA  X 1 . f Ml Nl .2MIN 1 ,J  A . I PLO  T. 

.  IPkINT) 

P  An a  ME  T  t  R  NC  S  =  7 1  «Kl=2*NCS 
p ahameter  incvm  -5a 
PARAMETER  IZE=C.I£=1 
P  A «A  ME  Tt  R  NI =  5  . N G  =  0 . N OP  =  7 

D  IMtNSION  AICCAX(INCTM).P<3.^.NCS).T(3.5.NCS).XN0RM<NCS).YK(2.NCS). 
A  Y  NOKMNC  S>  .Z  NORM  (NCS  >  ,  AKMIN  (  INI  YM  )  .  oKMAXUNCYM  ), 


a  fakfi  I  N  (  1  NC  YM  ) 

D  IMcNS ION  CA  HD (!63eA ) 

0  IMENSIOh  YL  (ID  .  2  L  (  1  0  ) 
c  cmmon  card 

C  LMM  ON/  f  AC  D  A  T/  P 

2  £RO=u.U 
0 NE= 1  . 

L  C  T  R  =  0 

D  RC  =  ACOS  <-1.  )/U>C. 

C 

C  l-ENERAL  PREPERATION  OF  OATA 

c 

A  2R=  A2  *L  RC 
E  LR=EL*DRC 

r  oll a  =  rolled  rc 

S  A2=S1N( A2R) 

C  AZ=COS<A2R) 

S  tL=SIN(ELP) 

C  EL=COS (ELP) 

S  RG=S IN ( ROLL  R  ) 

C  kO=COS  <  ROLL  R  ) 

Y  MAX=-1u.E1Q 

Y  MlN*10.E10 

2  MX  =-1  U.E  10 

2HN  =  10.Eir 

1 t22  F0RMAT(1X.I5.12F10.2> 

C 

C  kOTA  T  E  TARGE  T 
C 

DC  10  K  =  1  ,NC  S 
DO  10  J  =  1  .  A 

T  (1.J,K)aP(1.J.K)*CeL*CA2-Pi2. 
T  <2t  Jf  K  >*PM  .  J  •  k  Y*/CRu*SA2*LR0 
1SAZ)-P<D.J.N)»SK0«CEL 
T  (3.J.X)sP(1.J.a)«(SRO»SA2-(.RO 
1  *$  RG*  C  A2  )  ♦  P  (  1 .  J  .  K)  •  C  RO  *C  t  l 

I  F i T  1 2 , j  ,  k Y . G  T  .  v  -  A  X  Y  Y  M  A  X  *  T ( 2. 


,K)*CFL*SA2«P(3,J  .  K)  »  SE  L 
SEL*CA2)*P(2,J,K)*(C«0*CA2-SkO*SeL* 

SEL*CA2)»P(t.J.K)*(CrO*SEL»SA2 
*K  > 
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t  HHc.J.Ul.LT.KHN)  VPINMti.J.O 
t  K t < J , J  .K ) .  faT  .ZNAX )  l MAX  =  r<3.J.K) 

I  MUi.j.K'.LT.ZHN'  Z*IN=I<3,J,K1 
1  o  c  cn  r  i  u u  t 
c 

C  i  A  v  t  FIRST  YM  —  AN  0  Z*l--  V*t-UtS 
L  CTK*LC  TR«  1 
I  F  ( J  A  .  N  t  .1)  ttj  to 

Y  A  AX  1  =  1  h  AY 
f  MM  Is  V*  IN 

1  FA  A  1  =  2r  AX 

2  flNl<2NlN 
12  C  ON T I  MJc 

c 

C  UiPFU  It  SURFACE  NORMALS  , 

C 

DO  15  Ksl.NCS 

x  NOR PUi=(T<2,1. A) -TC2, 2. KH*{r<3,t,»0-T<3, A, K»-<T(2.1,K>- 
1  T(2.A.k  ))*m3,1  |K)-T<i,2,»n 

YNOHW(K)  =  <T(3,1.K)-T<3,2.K))*(TC1.1.K)-T(1,A.K))-tTC3,1.K) 

1  T(3«4,K))*(T(1,1*K)-U1.2.iO> 

2NORF'<K)=<T<1,1,K)-TM.2,K))*<T<2,1,K)-T»2,H,Kl)-<Tn»1,IC>  - 

1  T(1.4,K))*<r<2,1.lO-TU.2.K>> 

15  C  ONT INUk 
C 

C  COPPU  TE  I  Y  P  A  X  AN  b  IZF'A* 

C 

L  CTK=LC  TR* 1 
R  ZF'AXsAbSCZM  AX-ZMN) 

R  YMA  X  =  AbS(VNAX-»PjM 
I  ZPAX=RZPAX 
I YPAX=RYPAX 
C 

C  PREPA  re  plot  page 

c 

I  F(IPLOT  .EQ.C)  bC  TO  19 
C  ALL  TMPPLT(V) 

AXLOTh*  AP  Ax  1  <  (  Z  PA  X  -ZP1N  1.IYPAX  -YFJN  M 
AXLtTb  =  AlNT(AXLGTH/2.)  ♦  ID 
X  CNTR  =  A InT ( (  YPAX+YPIN)/2) 

Y  CNTR  =  AINT((2flAXtZPIN)/2) 

X  CRto  =  xCNTR-A  XLtTh 

Y  tfit  =  Y  C  N  TR  -A XLtTF 
X  tND=ACr*Tfi*AXLl>TF 

Y  fcNb  syCNTR+A XLGTH 
C  ALL  btUHLT-1) 

C  ALL  NOdROR 
C  ALL  F  E  1  &H  T  (  .1) 

s NCOOE <00,21 l.LauEL) At ,EL ,RvLL 

CALL  ftUEaAbtL.U’,'  '.1»*  6.1 

C  aLl  Gft«F<XOFto,5.XtND.YwKG,3,Yt,xO) 

C  eLL  HAk KE  R< i  ) 

C  all  SCLP1C  (  i.jc) 

.1C  f  law  A  T  <  *  AZ  S  '  ,  fa  ,  I  ,  it  ,  'fL*'  .F  F,  .  1  ,  ?X  ,'ROIL*  ',fd  .  1  .  '*  ' 
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4Mfc  V 


1  C  ON  1  III  LL 

C 

C  UAh  T  iNle  x  FOR  Z-KAaTt* 

t 

n  i«C  T  i*  =  f  L  0  A  T  (  I  N  C  V  f>‘  ) 

C  L»  =  KYf*AX/R*C»H 
D  lZa  =  nZi*ax  /flHtlx 

1-1  Fukf-AT<*  PLT1.  PLT2  . '26X  . 'YMN.'.  1oX  . 'ZwIN' .1  4X  .  'Ol  Y  .  '  .  luX  . 

V  0  L  Z  A  •  fi  X  .  1  Z  M  A  A  .  •  1  A  •  1  Y  PA  X  * / ) 

1  -C  f  ck*AT(oF16.  5. ill./) 
x  rA  X  =  u 
K  r  I N  =  U 
S  tcU  =  1  • 

I  sEtD  =  1 

•  h 1 T fc ( 6 •  1222  )  LCTw 

DC  ICO  I  Z3 1 »  I  nC  V  ► 

Z  tAh  =  IZ  »DLZA  «ZMI N 
C 

C  r  I  Ni>  T  INTERSECTIONS 
C 

L  VE=C 

A  KFAX<IZ  >s-10.E1u 
A  APIMIZ  JalO.MC 

251  F  C»AAT(*  Y.  ZbAR.  AKPIN(IZ).  A  X  H  AX  C  I Z  )  .  D  Y  P  I  N  ,  Z  Y  M I  N  l  INC  Y  )  .  D  Y  HA  X  . 
.Z  yTEPP,mAX<INCY).'.20X.'IZ.1.  lFLG.IAKFlb(INCY>.INCY'./) 

DC  «.C  I*  1  ,NC  S 
nd.I):  YPIN-100  . 

I  KXSORMI  )  .Lt  .0.)  60  TO  4u 
N  t  K  3  0 

DO  2b  X  =  1.  A 
K  fc  =  K ♦  1 

I  H*  .EQ  .4)  K  E  =  1 

I  F<AoS< T ( 3.K  , I )-T( 3,K E , H ) .l E.  1  .E-6>  GO  TO  25 
Y=<T(2.AE.I)-T(2.A,I))*(ZeAh-TC3.K,I))/(T(i.KE.lY-T<3,K,I))* 

1  TC2.K.I  ) 

X=<T(1,AEtI)-T(1.X.I))*(Z6An-T(i.K,I)W(T(3.KE,l)-Tl!,K,I))* 

A  T (  1 ,K  f I) 

I  NCY=< Y-YMIN  )  / DL  Y 

C 

C  IS  IN  TfcKSECTION  I.ITHIN  aOUNOS  UF  TEE  SURFACE 
C 

t  F(V .G  T .TI2.A.I) . AND . Y . to  T . T (  2  .  A  E  <  I))  GO  TO  25 
I  F(Y .LT.T(2.A.I)  .ANO.Y  .L  I  .  T ( 2 . K E  .  I  ) )  GO  TO  25 
I  FWeAR  .LT.T  (i.K  .1  J.AND  .ZdAk  .IT  .T(3.*E  .  I  >  >  GO  TO  25 
I  f YZbAR .GT.T <3.A ,1 ' .AND ,ZBA« .GT  .TT3.KE .1 ')  GO  TO  25 
1  F  <  V  ,LE  .AKPAXU*  >)  GO  TO  1v> 

»  M»A  X  (  I  Z  )  a  V 

1*5  I  F(Y  .GE  .AKPI  Mil  ))  00  Tu  Ivc 
A  AMN(  I  Z>  =  Y 
1*6  C  ONT INUE 
2 C  C  ONT INUE 
<5  ,C  ONT  INUE 
4'T  C  C,.T  INUE 

c 


C  ^  tli  N  C  I  b  t  TO  A  N  F  —  “  (  I  *  ) 

C 

c 

C  H_0  I  K'lhTS  y  1_  (  I  > 

r 

1  MIHLOT  ,E0.  ,  ■*  (j  U  TO  c,  5 

2  L( I  >  =  ZO  Ak 
/  L  (2  )  *  Z  a  A  k 

m  hEN ( AK* AX  (  I  n.LT.kLTI  « 0  H  .  A  x  F  A  x  (IZ).GT.HLTi) 

.  ONLEiS(A<FIN<IZ>.LT.HLT1.0H.AF*INU2).bT.PLT2> 
.  .  YL ( 1 )  =  AX.F  I N (  12  ) 

.  .  KFiIN  =  KFIN*1 

.  .  CALL  C  Oh  V  E (YL.ZL.  1.-1  ) 

.  ...MN 

...TIN  ‘ 

E  Lit 

.  -HEN(AKMIN(I2).LT.PLT1.0h.AKMlN(I2).OT.PLT2) 

.  .  YL  (  1  A*FAX  < IZ  > 

.  .  KF:AX=KhAX*1 

.  .  CALL  COhVE (YL.ZL  .  1.-1) 

.  ...FIN 
.  ELSE 

.  .  YL(1)=AAMAX(I2) 

.  .  Yl(2)-AKFIIN(I2) 

.  .  K  P  A  X  =<  FI  AX  ♦  1 

.  .  tMN  =  Kf-lN*  1 

.  .  CALL  COHVE  (YL.ZL,  2.-1  > 

.  ...FIN 
•  ..FIN 
On  C  ONT  lMJt 

o5  c  Cl.  r  i  no c 

100  C  UNT INOt 

L  LTH=LC  T  R  ♦  1 
J  MN=G 
J  F  A  X  =  u 
C 

C  STAhT  index  for  y-kaster 

c 

L  FAX  =0 
L  h IH  *u 

m  kITE(6,1222  )  LCTR 
0  C  iCO  I  =1,  INCVF1 
Y  t  Art  s  I  «  0  L  Y  ♦  YlMN 
C 

C  FIND  l  INTERSECT  IoNS 
C 

a  XpA  X(I 

exF'IMI  )  =  1i).e1o 
OOALU  J  =  1 ,N  CS 

I  F(X  NOHFt  IJ  Y  .  L  t  .0  .  )  00  To  AOv 
DO  il.G  h  =  1  .A 
K  t  =  X ♦  1 

I  F(X  ,t  3  .  A)  K  E  =  1 

I  FUli  (  T  <i  ,K  ,  J  )- T  (  2  .ft  E  ,  J  )  >  .L  E  .  1  ,f-6>  00  TO  i  "0 


AKf«- -»1  >=HuK  IZOFTaL  LIMtTS  FOr  S  E  G  0  F  nT I  Al  i  SCAN 
o*n- -<J  UvESTIC  AL  LIMITS  FUR  StGUENTUL  Y  SCAN 

CEM  tK  li«Av;E  RllH  RESPECT  TO  FIRST  IMAGE  ANb  USE  GHlu  INTERVALS  OF 
FUST  1!*  SlAN  IHaGE  bOTTOP  TO  TOP  (I)  A  NO  LEFT  TO  RIGHT  (J)  AT 
Dhc  RtTE  Intervals. 

CONVERT  HA*.  AN  L  MN.  EObE  COORDINATES  TO  GRID  INTEGERS* 

b  tYMN=YMN- Yl*  IN  1 
D  E  Y  .■>  A  X  *  ry  AX-  YhAX  1 

CtL»*<  wE*MlN«b£»MA)M/2. 

»  FlN®lf*INl«DcL» 
b  LYl*A0S<YHAXl-YMlNl)/RNCYH 
D  cZMN*2HIN-ZHIN  1 
0  E2PAx  =  2F  AX-ZHAX  1  * 

DEL2=C  DE2HI N«OEiHAX)y2. 

2  H1N  =  2P1N1*0  ELZ 
0L2A1*AUS<2HA*1-2M1N1)/RNC»M 
I NCThsC 

b  0  lv.5  0  1*1.  1  h  C  Y  H 
F  *=(aaMAX(I)-YPIN)/0Ly1 
I  X*  l  U  X  (  F  X  Y 

G  X* ( AaP I N( I ) -YPIM/OLYl 

1  G* 1 F I  *  (GX) 

2  A  CHARACTER  FLAG 
I  R  F  =  0 

C  CNTRAST  PIXEL  FLAG 
I  tF*l 

D  L  1  Co1')  J  *  1  »  INCYH 
F  Y*(bAP»AX(J)-ZMN)/DLZA1 
J  F*I F IX ( F Y  ) 

G  Y=<bK*IN<J)-ZMIN)/0LZA1 
J  G*I F IX (GY  ) 

I  R  =< I -IT*  I  NC  Y  H  *  J 

I  FdR.Nt  .0)  GO  TO  103  j 
I  R  =  2A 
I  RF  =  1 

1  l30  c  ont INUE 

I  F(IX.Nc.J)  GO  TO  1033 
CakD ( I  A  )*0N  E 
I  EF*  1 

GOTO  1  0  A  4 
1  i-33  C  ONT  INUE 

CAR  D ( I R ) *  ZE  NO 
I  F(1G.NE  .J )  GO  TO  1036 
CAhD(IR'*ONE 
I  E  F  =  1 

I  NCTR  =  INCTRt  1 
GOTO  1  L  A  4 
1  i36  C  CNT INUE 

CAKD(Ih)=ZEAo 
I  F <  J F  ,Nt  .1  )  Go  TO  1  34  o 
CAkCUR)*ONE 


l  FMTCi.kE.Jl-Td  <N.J))*(Vt>»R-T(!:.K<J))/(Td.KE.J)-l(Z.K.J))p 

A 

C 

l  I  S  INTERSECTION  w  I  1 h  I  N  □ 0  U  N  U  i  Of  Th  t  SURFACE 
>. 

1  f  (i  F  .  L  T  ,  T  (  i  .  *  .  J  )  .AnD  •  Z  f  •  L  T  .  T  (  5  , t .  J  ))  up  TO  10  j 

I  Mi  F  .0  I  .T  t  3  ,N  ,  J  >  .  AND  .ZF  .GT  .T  <  2  ,F  E  ,  J  ')  bO  TO  300 

I  HVtAR.LT.T  (  i  .  K  .  J  ).AND.YbAr(.GT  •  T  (  2  •  KE  .  J  )  )  bO  TO  lOu 

1  F(  Yl«  A  .LT  .T  l  i  .  J  )  .  AN  D  .  YBAh  .L  T  .T  (2  .  Kt  .  J  )  )  oO  TO  3Pu 

lFdF.Lc.bK*AX<I  ))  GO  TO  £j 0 
C  Ki*’n  X  (  I  )=ZF 
L  fi.IHfii.Hl 
i5  n  C  u.T  I'iUt 

I  M*  F  .fat  .BKftlNf  I  >>  GO  TO  cj  5 
bM’1N(I  )  =  Z  F 
L  |-IN  =  L*IN*  1 
.55  C  LNT INUc 
1( C  C  LNT iNUt 
ALP  C  ONT  IMJ  t 
C 

C  ADO  KAhi/Of  NOISE  TO  BKM--(Ii) 

C 

c 

C  PLOT  FLINTS 

c 

I  F<lf>LOT  .EC.  c>  00  TO  *E  5 

V  L(fc)-TdAH 

Y  L  ( 1  )  =  T  u  A  k 

4  hkH  (bKWAX(I  )  .L T .PL T3 .uR  .bkPA X <  I  J.GT.FLTA) 

.  UhLESS<bKMM  I  )«LT.PLT3.0R  .BKMIN<I  J.GT.plTA' 

.  .  ZL ( 1 )»rM*IN< I  ) 

.  .  JMNsJ  MN+  1 

.  .  CALL  COHVt  (YL.ZL.  1,-1) 

.  ...FIN 
.  ..FIN 
E  Lot 

.  hhN  (PKMN(l  ).lT.PLTi.uR.tiKMlN<I  F.GT.PLTh) 

.  .  ZL  t  1  YstKF  AM  I  ) 

.  .  JMAX=J  F  AX-*  1 

.  .  CALL  CURVE (YL.ZL.  1.-1) 

.  ...fin 

.  cLSE 

.  .  ZL(1)stAPAX(I  ) 

.  .  ZL  (2>*LAMN(  I  ) 

.  .  J  i'i  A  X  =  J  R  A  A  ♦  1 

.  .  JMh  =  JFIK‘1 

.  .  call  cukve(yl.zl.  2 .  -  i ) 

.  ...FIN 

.  ..Flu 

-C-  C  Lf.  T  lMJc 

-  j  5  C  U.  i  1  nUt 

-  *  C.  Li>  I  It.Ul 

L  LTP-LCTApI 
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t  C  r  u  1  U  4  4 

1  <•  n  c  cf*  r  i  nu  c 

Un  0  <  I  k ' 5  ZE  MU 
I  MJC  •  M t  .1  )  bO  TO  M-J 
CAmCi(Ih)*  ON  £ 

I  tf  =  1 

u  0  TO  10  44 
1  ' 4 5  C  uM INUt 

t  A  H  C  (  I  R  )  s  Z  E Ku 
1  '  -4  C  CM  IMJE 
I  >6C  C  CM  INUt 
1  (.50  C  CM  Ii.Ut 

L  CTk  =LC  T  »♦  1 

I  KJF'MhT.Ea  ,v)  bO  TO  110 
P  hlN  T  2  0  0  *  A  Z  • t  L  t POLL 
C  mLL  tMiPL(Q)  ; 

L  CTk=LCTR*  1 
'1C  C  OkflNUE 

L  CTk=LCTR«  1 
«  E  TO  h 

iuC  f Oku  A  T ( 1 H 1 •  5*. 'AZIMUTH  A  NS L£ * ' ,F 8 . 2 . / . 5X . 

A  'ELEVATION  ANCLE*'. fd .*./ . .  'ROLL  AN bL E  =  ' . f 8 .2 . / /  i 
c  NO 
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:>  u  a  P  u  L  T  INF  VliOucLf^filNi  <KOT  »INDm*NUDJ 
C 

C  VdOUeL  CONVEkTS  An  INPUT  INXN)  A  h  h  A  V  IUTO  A  (  „NA  2n )  A  r.  R  A  y  «y  ADDING 
*.  BACKGROUND  DATA, 

C  ARlNMNPuT  ARRkV,  *R0T=0uTPUT  ARRaV,  lNDrt=  INPUT  DI MENTION 

C 

DIMENSION  ARIN(INDA).  AROT(MOD) 

NOD l  =  ic  »  I N  DM 
NOD  *  a  *  l  N  D  P 
FD*  =  F  UOAT  (  INDI") 
k D M  =  in R T ( F 0  *  ) 

I  DR  =  I F I k ( HD" > 

IDRi*i*IDn 

:u  F  OR  (•  AT  (  zx  .  *  I  DR  =  I5.J> 

l 

L  J  =  Aui.  InDfck  FOR  OuTPUT  ARRAY  i*column  index 
i 

CO  juU  J  =  1.It>R2 
IF(J.GT.IDR)  go  TO  27C 
DC  iUG  1*1, IOR 
IJ=  IDR2*( J-1 MI 
I  JK  =  IDR*(J-1  MI 
APO  T  <  I  J  )  =  Aft  IN  (  I JK> 

iuO  continue 

c 

L  FILL  REMAINDER  uF  ROw  WITH  ZEROS 
C 

c2c  DO  2bU  1*1,  IUR 

I  J  =  luR2*<  J-1  MIDR»  I 

AROTUJ  )*0. 

25u  continue 
GO  To  JUG 
27 L  CONTINUE 
C 

C  FILL  REMAINDER  OF  RO«S  RlTH  ZEROS 
C 

DO  ioO  I*  1 , I  OR  2 
IJ*  IoR2*<  J-1  MI 
AROT(IJ)*L, 

EBU  CONTINUE 
ii)u  CONTINUt 
RETURN 
END 
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THIS  SUBROUTINE  CALCULATES  THE  CORRELATION  BETWEEN 

THE  A  VECTOR (OFT  C OE F f 1 C I EN TS )  AND  THE  B  VECTOR<DFT  COEFFICIENTS) 


SUBROUTINE  C AL C OR ( KK 2  .  J J 2 . NV  ,  A Z  .  EL  .R OL L . J  .N1 2 . N 22  > 

DIMENSION  WH(163B4)  ,WHR  (16384  ) 

DIMENSION  SM2B),1NVM2S>,»'3) 

DIMENSION  CMAX(20) 

DIMENSION  MAXX ( 20) .MAX Y (20) 

DIMENSION  w<12B,12a) 

C  OM  PL  E  X  *8  AM6i<i4)  .e(l63b4> 

COMMON  WHR.  WH.A.CMAX 
M ( 1  )  =KK  2 

M  ( 2  )  =  J  J  2  , 

M(3  >*0 

2037  FORMAT(7.5X.'J  =  ' . I  2, 3X . 'CMAX  =',F17.6./> 

63  F0RMAT(5X.'N12  ='.I4,'N22  ='.I4.'NV  *'.I4> 

C 

C  DOUBLE  SIZE  OF  DATA  ARRAY 

C  WHR  a  INPUT  VECTOR.  WH  *  OUTPUT  VECTOR.  NV  =  INPUT  DIMENSION 

C 

NV4 =  4  *N  V 

CALL  VDOUBL ( W  HR . WH . N V «  N V  4 ) 

C 

C  PLOT  DATA  ARRAY 

C 

C  TRANSFER  THE  WH  (K )  DATA  TO  THE  COMPLEX  8  MATRIX 

C 

NDA  TA  =  0 

DO  104  12*1 « N22 
DO  103  11=1. N12 
I WH  *  <I2-1)*N12fI1 
B  (I WH)*WH  CIWH) 

I F (  WH(IWH)  . G  T .0.0)  NDATA  =  ND ATA*  1 
103  CONTINUE 

I WH M=  I  W  P  -*9 

WRITE (6.72)  (WH (I ) . I=IWHM. IWH ) 

104  CONTINUE 

WRI TE (6,2  08)  ND  ATA 
20o  FORMATS  NDATA  =  '  »  1 7  ) 

I F( NOATA.Efl .0)  STOP 
C 

C  COMPUTE  THE  DISCRETE  FOURIER  TRANSFORM  OF  THE  wH  (  K)  DATA 

C 

71  F0RMAT(16(1X,F7.2)) 

72  FORMAT  (10(1X,E12.5)) 

73  FORMAT(7(2X. 15) ) 

CALL  HARM(B,M,INV,S  .1  .IPERR) 

C 

C  COMPLEX  MULTIPLY 


DO  220  11*1  . NV4 


0<I  1)  =  A  (  ID  *CON  J6(b  (  I  1  )  ) 

I  F  (  I  1  .LT.N22)  WRITE(t>  .101  >  1 1 . A  (  II ) . 8 ( II  ) 

2  20  continue 

c 

c  inverse  Fourier  transform 

c 

CALL  HARMIB.M.INV.S  .-1.IPERR) 

c 

C  CALCULATE  THE  MAGNITUDE  OF  THE  FOURIER  COMPONENTS 

C  AND  STORE  ONLY  THE  POS  FREQ  VALUES  OF  V<I1,I2) 

C 

DO  20  I  2=1, N22 
00  3U  11=1. N12 
I WH  =  ( I  2-1 >  *Nl 2 ♦  1 1 
uM  1,I2)=CABS(B  (  IVH  )) 

I F ( W ( 1 1 . 1  2 ) .LT.CMAX(J  ))  60  TO  20 
CMAX(J)=W(I1.I2>  . 

M AX  X ( J ) =1 1 
MAXY(J>=I  2 
30  CONTINUE 
C 

C  PRINT  THE  OFT  COEFFICIENTS 

C 

IF(MAXX(J ) • G  T  .  3  )  GO  TO  40 
MM3  =1 
MM2=2 
MM1=3 
MAX  X  (J  )  =4 
MP1*5 
MP2=6 
MP3  =  7 
MP4  *b 
GO  TO  42 
40  CONTINUE 
N 1 2  M  =  N1 2-4 

IF(MAXX(J  ).GT.N12M)  GO  TO  41 
MM3=MAXX(J)-3 
MM2  =M  A X  X ( J  )  - 2 
MM1 *MAXX<J)-1 
MP1 =MAX  X ( J ) ♦  1 
MP2  =M A X  X ( J ) ♦ 2 
MP3=MAXX(J)»3 
MP4*MAXX( J)*4 
GO  TO  42 
41  CONTINUE 
MM3=N12 -7 
MM2  =  N 1 2 -6 
MM1 =Nl2-5 
MAX  X ( J ) =N 12 -4 
MP1 =N12 -3 
MP2=Nl2-2 
MP3=N12-1 
MP4*N12 
42  CONTINUE 
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fcfil  Tt  (6.11.3)  HHi.HM2.)*l«1,MAXX  |J>,MP1,MP2,MP3,I*PA 
1  U  fOMNir  K'.5X.'A<'.l3.'.K)*.?<6X.'A<'  .13. '.K)')) 

ini  fORMAT<2X.U,8(lx,t12.S)) 

00  61  12*1, N22 

WRITE(6.101)I2«(W(I1,I2),I1sMM3,MP4) 

6 1  CONTINUE 
RETURN 
END 
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c 

C  UAL  IS  A  RADAR  RANGE  ESTIMATOR  USING  A  K  ALP  AN  EILTER. 

C  THERE  ARE  2  RANGE  MEASUREMENTS  AND  A  RADIAL  ACCELERATION  ESTIMATION 
C  MEASUREMENT. 

C 

C  MODEL 

C  X(K*1)  *  H(K)*X(K)«G(K)*U(K) 

C  Y  (  K  ♦  1  )  =  S(K*1 )*X(K*1 )*V(K*1  ) 

C  KALMAN  ESTIMATE 
C  X  * ( K ♦ 1 )  *  H(K)*X-HAT(K) 

C  X  -M  A  T  (  K  ♦  1  )  »  X'(K)-K(K4l)*(S(K*1)*X'(K)-Y (K*1) ) 

C  K(K*1)  =  P'U*1>«ST(K  +  1)*(S(K*1)*P'(K*1>*ST(Kn)*R(K*1))«*-1 
C  P'CK+1)  *  H(K)*P(K)*HT(K)*6(K)»Q(K)*GT(K) 

C  P  (  K  ♦  1  )  »  P' ( K* 1 ) -K (K* 1 ) * S ( K* 1 ) • P"  < *♦ 1 ) 

C  PIG)  a  6 (0 ) *0 ( 0 ) *GT (0) 

C  T  =  TRANSPOSE 
C  SUBROUTINES  USED:  INTCON 
C  MANVR 

C  ETKALI 

DIMENSION  IU),h(4|0,GU)iS(3i4)  ,P(4,4),T1(4,4),T2(4,4),T3(3), 
•Tx(4),Y(3),R(3,3),E6(4,4) 

DIMENSION  TP  1 100)  ,EP1  Cl  00)  ,XP1  (TOO)  .RNOISUOO)  tVPl  OOO)  *  VP?  (100)  , 
•RN0IS2C100)  ,XTP(100),2TP(100) 

DIMENSION  API (100) . AP2I100) ,AP3 (100) 

DIMENSION  WHEN(2> ,TMEOD(?) 

DIMENSION  PAUV(3> , ANUV(3) ,ANL(3) , ANLR(3> ,TPA(3) 

DIMENSION  I  OPT ( 5 )  ,ST  AT ( 5  > 

DIMENSION  IPAK(ISO) 

COMMON  XT,YT.2T,VXT,VYT,VZT,VEL0C,T1ANG.T2AN6,DELR,1G,TRAT1, 
.DELT.NPRf ,TRAT2,II,NPH,R90,I62.ICTS.PI.IR.lH,IOS 
COMMON  /  1 N  T  C  /  AZ .EL.ROLl .R0L2.R0LA  ,A2DE6,ELDE6,R0L1D6  .R0L2D6, 

•  R0L4D6  ,T6S1 ,T GS 2 ,RT , R DOT T , R 2D0TT ,  D T , TR A D 1 . TR A D 2 . R AN6T  . 

•  RANGTM.XR, YR,ZR,WxTM,VVTM,VZTM,VR2D  , AXT, AYT ,AZT, AXT6 .AYTG.A2T6, 

•  ANA  ,ANE , PAUV 
OATA  IN.Nf  I  T / 3*  4 / 

DATA  IS,  "1/2*3/ 

DATA  1L  ,L/?*1 / 

DATA  ISEED/42573/ 

DATA  PI/3.141592654/ 

C  1/(S**2)  MODEL  STATE  VECTOR 

CALL  ER80ID('  6220  R.H.  WILLIAMS  6262  ') 

NI*5 
N0  =  6 

T6S 1=10. 

TGS2=10. 

DT*0.05 

NP*31 

NPMsNP-1 

NPHsNP/10 

RES*0.5 

R90aPI/2. 

R270*3.*PI/2. 

6YD*10.7252 
NRUN*0 
10  CONTINUE 
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3-0  TARGET  TRAJECTORY  DATA 
ORIGIN  AT  INITIAL  TARGET  POSITION 
RT  =  TRUE  RANGE  RELATIVE  TO  RADAR 
RDOTT  *  TRUE  VELOCITY  RELATIVE  TO  RADAR 
RPDOTT  =  TRUE  ACCELERATION  RELATIVE  TO  RADAR 
VELOC  *  TARGET  VELOCITY  RELATIVE  TO  ORIGIN 
TRAD-  ■  TURN  RADIUS  OF  T6S-  6.  TURN 
TRAT-  «  TURN  RATE  OF  T6S-  6.  TURN  (RAD/SEC) 

-R  *  RADAR  LOCATION  AT  T  C  0 ) 

-T  *  TRUF  X.Y  ,2  TARGFT  POSITION 
V-T  *  TRUE  X  »  Y  ,  Z  VELOCITY 
A-T  *  TRUE  X.Y, 2  ACCELERATION 
A-TG  *  TRUE  X ,  V  ,  2  ACCELERATION  <6'S> 

ANA  b  VERTICAL  <A7)  ACCELERATION  RELATIVE  TO  TARGET  ORIENTATION 

ANE  »  HORIZONTAL  <£L>  ACCELERATION  RELATIVE  TO  TARGET  ORIENTATION 

PAUV(I)  =  PITCH  AXIS  UNIT  VECTOR 

PAUV  < 1 )  *  X 

PAUV<2)  *  2 

PAUVC3)  *  Y 

ATC  *  TIME  CONSTANT  OF  ACCELERATION 

TCK  =  TRANSITION  MATRIX  ACCELERATION  TI»e  CONSTANT 
ATC  *  2. 

TC  *  ATC/DT 

TCK  *  < 1 .-EXPi-TC  )  ) 

Q  *  COVARIANCE  MATRIX  OF  U  (LXL>  -  INPUT 
*  ECU*UT]  »  (RMS  RANGE/SEC*SEC*SEC  )**? 

REAO(5.150>  q,rcv,ilast 
DO  115  10S*1  ,2 
IFCI0S.EQ.2)  DTbO.1 
DO  118  I  R  » I  ,3 
DO  116  IH=1,2 

H  *  STATE  TRANSITION  MATRIX  ( NX  N ) 

DATA  H/ 1 6  »0  •  / 

A6MTK  =  GAMMA  ACCELERATION  TRANSITION  MATRIX  CONSTANT 
ARTK  s  NORMAL  RADIAL  LOAD  ACCEL.  TRANS.  MATRIX  CONSTANT 
IF(IH.EO.I)  AGMTKbI.O 
1FUH.E0.2)  AGMTKbO.O 
IF(1H.EQ.3)  AGMTK  «0.5 
IF(IH.EQ.I)  ARTKbO.O 
IE(1H.EQ.2)  ARTKbI.O 
IF(1H.E0.3)  ARTn*0,5 

H(1,2)*DT 

M<1,3>«AGMTK*0T«0T/2. 

H  < 1  ,A)=ARTK*DT*DT/2. 

H(2,2)*1. 
h(2,3)*DT*AGmtk 
m  <2 ,0*ARTK  *DT 
H(3,3)*l. 

ANLRK  *  RAOIAL  NORMAL  LOAD  ACCELERATION  TRANSITION  CONSTANT 
ANLRK  *1  . 

M ( A , A ) *  ANLR  K 

G  *  STATE  NOISE  INPUT  MATRIX  ( NX  L ) 
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300  FORMAT  <  *  0  R  *',F9.3,'  RT  *',F10.1.'  ROOTT  *',  1 

*4(F7.1).I3) 

CALL  DAT£(WHEN> 

CALL  TOFDAYCTMEOD)  j 

UR1TE(N0*112)  WmEnM  >  ,whEN<2  >  ,TMEOD  <1  >  ,TMEOO  (2>  I 

112  F0RMAT(2A6.2X,2A6) 

WRITE(6,90> 

90  FORMAT ( *  X',4X.'RT  ' , 7 X ♦ ' V ' , 3X  , 

*4X.'X<1),',7X.#X<2>',2X,'ERROR=RT-XO>‘’.2X,'NOISE'» 
*2X,'AXTG',2X,'AYTG',2X,'AZT6<',2X,'2ND  0 1  F  F-VELOC'  ,  /  ) 

WRITE (6*1 30 >  k,RT,Y(1),X(1),X<2),EP1<  1),RN0IS(1> 

C 

C  MAIN  LOOP 
C 

00  100  11*1, NPM 

IP*1I*1 

IPS*0 

IF(U.EO.I)  I  PS  =1 
PCTNS*0.7  1 

RNOIS(II)*SORT(R<1»1))*66NOF(ISEED)*PCTNS 

RN0IS2<1I)=SQRT(R(2,2))*GGN0F(ISEED)*PCTNS 

DELR  =  VELOC*DELT*FLOAT(NPR.’)  i 

C  MANVR  CALCULATES  TRUE  POSITIONS  (XT,YT,ZT>  ANO  TRUE  VELOCITIES  tVXT.j 
C  VYT,VZT>  TARGET  STARTS  9  ORIGIN  9  T'O.  AND  FLIES  TOWARD  RADAR  FIXED  i 
C  LOCATION  RT  ON  X  AXIS 

C  X  *  DOWN  RAN6E,  REFERENCED  TO  TARGET  9  T  *0 

C  T  *  UP  RANGE,  REFERENCED  TO  TARGET  9  T  *0 

C  Z  *  OFF  RANGE,  REFERENCED  TO  TARGET  9  T  *0 

CALL  MANVR 

C  V  *  TOTAL  TRUE  VELOCITY  RELATIVE  TO  COORD  SYSTEM  ,  T(0) 
V»SQRT(VXT*VXT*VYT*VYT*VZT*V2T) 

AXT*(VXT-VXTM)/(0ELT*FLOAT (NPRF)) 
AYT*(VYT-VVTM)/<DELT*FLOAT(NPRF>> 
AZT*<VZT-VZTM)/(DELT*FLOATCNPRF)> 

C  AT  *  TOTAL  TRUE  ACCELERATION  RELATIVE  TO  COORD  SYSTEM  ,  TCO) 

AT*SORT (AXT  *AXT*AYT*AYT^AZT*AZT) 

AXTG*AXT/10.7252 

AVTG*AYT/10,7252 

AZTG*AZT/10,7252 

ANE*<(AYT*VXT-VYT*AXT)*VXT-(AZT*VYT-VZT*AYT)*VZT)/(V*VH) 

ana*<azt*vzt-vzt*axt>/vh 

C  AM  *  HORIZONTAL  ACCELERATION  -  RELATIVE  TO  COORD.  SYS.,  T(0) 
am*$qrt<axt«axt*azt*azt> 
ele*ane*cos<el> 

ELN*SQRT(ANA*ANA+ELE*ELE> 

C  VM  *  HORIZONTAL  VELOCITY  -  RELATIVE  TO  COORD.  SYS.,  TCO) 

VH*$ORT(VXT*VXT*VZT*VZT) 

AZ*ATAN2(VZT,VXT) 

AZDE6*AZ* 180,/PI 

EL*ATAN2(VYT,VH) 

ELDEG*EL*180./P1 

anael*ana/ele 

C  1CTS  *  COORDINATED  turn  SWITCH 

IF(ICTS.EQ.I)  R0L4*ATAN2  < AN A,  10. 7252) 

R0L5  *ATAN?(ANA,10.7252) 
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C 


c 

c 

c 


c 

c 


c 

c 

c 

c 

c 


G(1)=DT*DT*DT/4. 

G(2)=DT*DT/2. 

G(3)*DT 
6(4)*  6(3) 

r  s  COVAK I  ANCE  MATRIX  Of  V  <*(1X1*1)  -  MEASUREMENT 
DATA  R / 9*0 • / 

R(1,1)=RCV 
R  (  2  »  2  )  =  R(1.1> 

A(3.3>*100*R<1»1> 

S  »  OBSERVER  MATRIX  (**1XN)  T  =  SX 
DATA  S/12-0./ 

S(1.1)«1. 

S(2.1)*1. 

S(3»4)«1 • 

1NTCON  INITIL1 ZES  TRAJECTORY  VARIABLES 

CALL  INTCON 

TX  »  ITERATED  EXTENDED  THRESHOLDS 
TX(1)*1. 

TX  <2)*TX (1 ) 

TX<3)«TX<2> 

T  X  (  4  )  *T  X  (  3  ) 

K  *0 
T«0. 

LL  *0 
INCP*0 

RN0ISC1 )*SORT (R(1 *1))*66NQF (1SEED) 

RN0IS2 (  1)*S«RT(R<2»?)>*66N0f ( I  SEED) 

»  *  INPUT  OBSERVATION  VECTOR  <MlXl> 

MULTIPLE  MEASUREMENTS 
Y<1  )*RT*RN0IS<1 > 
t<2)»RT*RN0lS2<1) 

Y(3)*0. 

PRESET  ESTIMATOR  INITIAL  STATE 
X(1)  *  RANfiE(K) 

X ( 2 )  -  RAN6E  RATE(K) 

X ( 3 )  *  RAN6E  ACCELERATION 

X<4)  «  ESTIMATED  RADIAL  NORMAL  LOAD  ACCELERATION 
X100FF*  1. 

X20n=1 .1 

X(1)*T<1)*X100FF 
X ( 2  >*X  20*  *R  OOTT 
X ( 3)*0. 

X ( 4 ) *0  • 

EPl (  1)*  RT-X(1) 

XP1 < 1 ) *X ( 1 ) 

VPl(1)*X(2) 

VP2(1 )«ROOTT 
API (1 )»X (3) 

AP2(1)«0. 

AP3(1 )*0. 

EPMAXbO . 

RDRXBRT 

nrun*nrun*i  ,,  yBlla 

HR  I  TE ( 6  *  300 )  9.RCV.RT, RDOTT  ,A6MTX  ,  A  RT*  ,  S  <  3 . 3  )  ♦  N  RUN 
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1U  FORMATC'  COOO.  TURN  SU  *'13.'  R0L4  =',F9.3.'  COORD.  TURN  ROLL 
*',F9.3> 

ROLOEG*ROL<.*1bO./Pl 

ANAOOT*( <~VXT*AXT-VZT*AZT)*ANA) / <VH*VH) 

AZDOT*ANA/VM 

eldot*ane/v 

ROLDOT*1 0.7252* AN ADOT 7(10.7252*1 0.725 2* AN A* ANA) 

C  V-TN  *  DELATED  TRUE  X.Y.Z  VELOCITY 

VXTM*VXT 
VVTM*VVT 
VZTM=V2T 

C  -3  *  X.Y.Z  DIFFERENCE  BETWEEN  RADAR  AND  TRUE  TARGET  POSITION 

X3*XR-XT 
Y3*YR-YT 
2  3*2  R-Z  T 

C  RAN6T  *  TRUE  RANGE  RANGTW  *  DELAYED  TRUE  RANGE 

RANGTM*RANGT 

RAN6T*SQRT  <  X  3 *  X  3*-Y  3 *  Y  3* 2  3  * 2  3  > 

VN01S*RN0IS(I1) 

RAN6N*R AN6T4VN0IS 
RAN6N2*RANGT4RM0IS2(II) 

RENDR*ANOD( RAN6N.RE S) 

RENDR2*AN0D (RAN6N2 ,RES> 

RES2*RES/2. 

C  RAN6E  *  MEASURED  RAN6E  WITH  NOISE  AND  QUANTIZED  TO  RESOLUTION  ' 

RANGE*R ANGN-REMOR  / 

RANGE2*RANGN2-RENDR2 
R  T *R AN6T 

C  AMNL  *  MAGNITUDE  Of  N0RNAL  LOAD  ACCELERATION  (  G'S  > 

AL*1 3 • 

BE*-3 . 

GAM*0.5 

EPS*66N0F(ISf£D) 

AMNL*AL*BE*EXP(6AM*EPS) 

C  AZ  *  AZDEG 
C  EL  *  -ELOEG 
C  ROLL  *  -ROLDEG 
C  PAUV(I)  *  X 
C  PAUVC2)  *  2 
C  PAUV  <  3 )  *  Y 

SAZ*SIN<  AZ) 

CAZ*COS(  AZ  ) 

SEL*SIN(-EL) 

C  E L  *C  OS (  -  EL  ) 

$R0*SIN(-R0L4) 

CR0*C0S (-ROLL) 

TPAC1)*PAUV(1)*CEL*CA2-PAUV(2)*CEL*SAZ4PAUVC3)*SEL 
TP A (2)*PAUV<1 )*(CRO*SAZ*SRO*SEL*C AZ ) ♦ P AU V ( 2 ) * ( C RO* C A 2 -SR0*S EL * S A 3 
,-PAUVC 3)*SR0*CEL 

TPA (3)«PAUV  <1 )*  <SRO*SAZ-CRO*SEL*C AZ )*PAUV( 2)* ICR0*SEL*SA24SR0*CA: 
.♦PAUV(3)*CR0*CEL 

C  ANUV(I)  *  NORMAL  acceleration  unit  vector 
C  *  <  TP A ( I )  X  V-T)/V  CVECTOR  CROSS  PRODUCT) 

C  (X.Z.Y) 

ANUV(1)*<(TPA(2)*VYT)  -  (TPA(3)*VZT))/V 
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ANUV(2)*( (TPA(3)*VXT)  -  (TPA(1)«VYT))/V 
ANUV(3)=( (TPA(1)*V2T)  -  (TPA(2)*VXT))/V 
C  ANL  (  1  )  *  NORMAL  LOAD  ACCELERATION 
ANL  Cl )  *  ANUV ( 1 ) *  AMNL 
A  NL  C  2 )  *  ANUV(2)*AMNL 
ANL  C  3 )  =  ANUV(?)* AM  NL 

C  ANLRCI)  =  RACIAL  NORMAL  LOAD  ACCELERATION!  G'S  ) 

C  s  PROJECTION  (DOT  SCALAR  PRODUCT)  OF  ANL(l)  ON  THE  RADIAL 

C  VECTOR  FROM  TARGET  TO  RADAR/RADIAL  RANGE 

ANLR(1)*((ANL(1)*x3)+(ANL(2)*23)« ( ANL C 3 ) *Y 3 > > /R AN6T 
V ( 1 ) *  RANGE 
Y(2)*RAN6E2 
V(3)*ANLR(1 >*GVD 

C  VR  2D  »  1ST  DIFFERANCE,  RANGE  VELOCITY  VR2DD  *  DELAYED  VR2D 
VR2DDSVR2D 

VR2DMRANGT-RAN6TM)  /<  CELT*  FLOAT  (NPRF)  ) 

C  AR  *  RANGE  ACCELERATION,  2ND  DIFFERANCE 
ARMsAR 

AR=(VR2D-VR2DD)/(DELT*FLOAT (NPRF)  ) 

WRITE(6,133)  RAN6T,RAN6TM,VR2DD,VR2D,AR,Y(3).X(3>, 

*X3,Y3,Z3 

C  TEST  RADIAL  2ND  DIFFERENCE:  AR,  &  LIMIT  TO  MAX.  EXPECTED 
C  RATE  OF  CHANGE:  SORT (Q ) 

ADIF«ABS(AR-ARM) 

0LIM=SQRT(Q)*DT 

IFCADIF.6T.QLIM)  AR=SI6N(QLIM,AR)*ARM 
C  TEST  FOR  RE-INITILIZATION  OF  KALMAN  FILTER 
SI6TH*2.0 

S1G1*SI6TH*SQRT(R(1,1)) 

SI62*SI6Th*SQRT(R(2,2>) 

S1G3*AMAX1(SI61,SI62) 

PM A  X  *0 • 

EEST»((Y(1)*Y(2))/2.)-X(1) 

I F ( E  E  ST  .LT.S163)  60  TO  44 

INCPMNCP^I 

IPS«1 

DO  42  J  P * 1  ,  N 

PMAXSAMAX1 (P(JP.JP)  ,  PM  A  X ) 

42  CONTINUE 
PPCT*1.0 

DO  43  J  P * 1  ,  N 

P( JP, JP)*PMAX*PPCT 

43  CONTINUE 

44  CONTINUE 

WRITE (6,133)  AZ , EL ,R0L4 , TP A ( 1 ) ,TPA(2) ,TPA(3) ,ANUV(1) , ANUV (2)  , 

. ANUV (3) ,AMNL, ANLRCI ) ,AR 

CALL  FTKALI (K,X,H,6,Y,S,Q,R  ,P , I N , I S , I L , N , Ml , L , T 1 , T 2 , I T , T3 , 
•TX.F6.IER, IPS) 

T*T«DT 

EP1 ( IP)*  RT-X(I) 

XP1 (IP)*X(1  ) 

VP1 (IP)*X(2) 

VP2(IP)*VR2D 
API (IP)*X(3) 

A  P2 ( I P ) *AR 
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;'Mk  V- 


*p3 ( ip) *r (3  > 

TP<1P)*T 
XTP(IP) =XT 
7TP(IP)=ZT 

VPCT*X<2>«OT*100./EPl<IP) 

APCT  =  X(3)<*DT*0T*5O./EP1(IP) 

PCTM=T2(1 .1 >*10Q./EP1 (IP) 

WRITE (6,134)  II,VPCT»APCT,PCTM,T2(1,1),PMAX 
134  F0RMAT(1X,I3,'  PC  T  VELOV,  ACC,  PEAS  * ' , 10 ( FI  2 . 3 ) ) 
IF(ABS(EPK1P)).GT.ABS(EPMAX))  £PMAX=EP1<1P) 
IF(ABS(EP1<IP)).GT,40.)  LL*LL*1 
K*K*1 

I  F  (  F  6  ( 1  .D.GE.1.)  GO  TO  51 
WNS*F6(2,1)/((1.-FG(1,1))*DT) 

IFCwNS.LT. 0.)  60  TO  51 
WN*SQRT (WNS  ) 

WNHZ*WN/?.*P1 

0R*(F6(1,1>-DT*F^(2,1>)/(2*(1.-F6(1 ,1 ))*OT*WN> 

194  FORMAT ( '  NATURAL  FREQ.  (HZ)  *',F9.2,'  DAMPING  RATIO  *'.F8.3) 

51  CONTINUE 

WR1TEC6.13Q)  K.RT ,Y(1 ) ,X  <1 ) ,X (2) ,£P1  C  I  P  )  .  RNOI  S  ( 1 1  )  ,AXT6,AyT6,A2Ti 
*VR?D 

100  CONTINUE 
C 

IOPT( 1 )*1 
I0PT(5)*1 
NPP=NP»1 

CALL  BEIU6R(EP1.NPP,I0PT,STAT,I£R) 

WRITE (6, 102)  STAT(1),STAT(5),EP«a* 

102  FORMAT!  '  ARITHMETIC  MEAN  OF  RANGE  ERR  *',F?.3, 

VARIANCE  CAVE.  RMS  ERROR)  OF  RANGE  ERROR  *',F9.3, 

MAXIMUM  RANGE  ERR  *',F9.3,/) 

WRITE (6,1 04 )  LL,INCP 

104  FORMAT!/,'  NO.  OF  RETURNS  OUT  OF  6 ATE  *',IA, 

•  '  NO.  TIMES  P  INCR.  *',I4> 

I R  PLOT* 1 

IFCIRPLOT.EQ.O)  60  TO  113 
CALL  INTAXS 
CALL  VAXANGCO.) 

CALL  SIMPLX 

CALL  TITLE!'  KALMAN  F ILTERS' ,100  , 

•  'ERROR  IN  TAROSS', 

.  100, 'TIME  IN  SEC. S', 100, 6. ,8.) 

CALL  XT1CKSC5) 

CALL  YTICKSC5) 

X0R6*AMIN1(ARMIN(£P1,NP),ARMIN(RN0IS.NP)) 

XMAX6*AMAX1 ( ARM AX  <EPl ,NP) ,ARMAX(RN01S,NP)) 

CALL  6RAF(X0RG,'SCALF',XMAXG, 

•  ARMINCTP  ,NP) ,'SC ALE' ,ARMAX (TP  ,NP)) 

CALL  CURVE (EP1 ,  TP, NP, 10) 

CALL  CURVE(RNOIS,TP,NP,10) 

CALL  MESSAG('COV  Q  «S', 100,1.,  ft. 9) 

CALL  RE  ALNO  <  Q , 0 ,' ABUT ',' ABU T ' ) 

CALL  ME  SS AG ( '  COV  R  *S',100,  'ABUT ', 'ABUT') 

CALL  RFALNOCR (1  ,1 ) ,2 , ' A  BUT ' ,'ABUT ') 
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CALL  MESSAG('  #  »t',  100,  ' APUT '  ,  ' ABUT ' ) 

CALL  1NTN0(NRUN,'ABUT','A8UT') 

CALL  DATE (WHEN) 

CALL  TOFDAV (TMEOD) 

CALL  MESSAG(WHEN,10,1.,9.3) 

CALL  MESS  AG (TMEOD, 10. 'ABUT', 'ABUT') 

IDUWHf =LINE ST ( IP AK ,150,60) 

CALL  LlNES('ERRORS',IPA»C,1) 

CALL  LINES('N01SES',IPAK.2) 

CALL  LEGENDdPAK  ,2, 4. 0,2.0) 

CALL  ENOPL(0) 

113  CONTINUE 
I WPLOT  *0 

IM1R  .E0.3)  1VPL0T»1 
I F ( IVPLOT .FQ.O)  60  TO  114 
CALL  1NTAXS 
CALL  YAXANG(0.) 

CALL  SIMPL* 

CALL  T I TL  E ( '  KALMAN  E I LTE R S' , 1 00 , 

'  VELOC  IN  YARDS/SECS'. 

100, 'TIME  IN  SEC. S', 100, 6. ,8.) 

CALL  ITICKS(S) 

CALL  VT1CkS(5) 

X0R6*AMIN1(ARMIN(WP1,NP),ARMIN<  VP2,NP)  ) 
XMAXG=AMAXl(ARMAX(VPl,NP),ARMAX(  VP2,NP)> 

CALL  GRAF(X0R6, 'SCALE', XMAX6, 

ARMINtTP  ,NP) , 'SCALE '  ,ARMAX (TP  ,N 
CALL  CURVE ( VP1  ,  TP,NP,lO> 

CALL  CURVE (VP2  »TP,NP,10) 

CALL  MESSAG('COV  0  *S', 100,1.,  6.9) 

CALL  RE  ALNO (  0 , 0 , 'ABUT ', 'ABUT '  ) 

CALL  ME  SS AG ( '  COV  R  *S',100,  'ABUT ', 'ABUT') 
CALL  REAL NO (R(1,1), 2, 'ABUT', 'ABUT') 

CALL  ME  SSA6 ( '  *  »S',  100,  'ABUT' , 'ABUT') 

CALL  1NTN0(NRUN, 'ABUT', 'ABUT') 

CALL  DATE (WHEN) 

CALL  TOFDAY (TMEOD) 

CALL  MESSAG(WHEN,10,1.,9.3) 

CALL  MESSAG( TMEOD, 10, 'ABUT', 'ABUT') 
IDUMMy*LlNEST( IPAk, 150,60) 

CALL  L1NES('X(?)S',IPAK,1) 

CALL  LINES('VR2DS',IPAK,2) 

CALL  LE6ENDUPAK  ,2, 5. 0,1.0) 

CALL  ENOPL(C) 

CONTINUE 
IAPL0T*1 

IF(IAPLOT.EQ.O)  60  TO  1146 
CALL  1NTAXS 
CALL  TAX ANG (0  .  ) 

CALL  SIMPLX 

CALL  T I TLE ( '  KALMAN  F  I  LT E R S '  ,  1  CO  , 

'  ACCEL  IN  YDS/SEC-SECS', 

100, 'TIME  IN  SEC. S', 100, 6., B.) 

CALL  XTICkS  (5) 

CALL  YTICkS(S) 


X0R6=AMIN1(ARM1N<AP1,NP),ARMIN<  AP2.NP)) 
XMAXG=A«AXl(ARMAX(AP1,NP),ARMAX(  A  p?  ,NP  )  ) 

CALL  GRAFCXORG, 'SCALE', X*AX6, 

*  ARNINOP  .HP) .'SCALE  '  ,ARNAX (TP  ,NP)) 
CALL  CURVEfAPI,  TP, NP, 10) 

CALL  CURVE<AP2.TP,NP,1C) 

CALL  CURVE(AP3,TP,NP,10> 

CALL  MESSAGC'COV  Q  =*',100,1.,  8.V) 

CALL  RE  ALNO  (  Q ,0 , 'AB UT ' , ' ABUT '  ) 

CALL  MESSAGC'  COV  R  =*',100.  'ABUT', 'ABUT') 
CALL  RE ALNO (R (1 , 1 ) . 2 , ' A BU T '  ,  ' ABUT ' ) 

CALL  NESSAGt'  «  =*',  100.  ' ABUT ', 'ABUT' ) 

CALL  INTNOCNRUN, 'ABUT', 'ABUT ') 

CALL  DATE(WHEN) 

CALL  TOFDAV (TMEOD) 

CALL  MESSAG(k»HfN,10.1..9.3) 

CALL  MESS AG (TMEOD, 10, 'ABUT', 'ABUT') 

IDUMMY*L  INF  ST  (IPA.K,  150,80) 

CALL  LlNES('X(3)S',IPAK  ,1  ) 

CALL  LINESC'AR  t',IPAK,2) 

CALL  LlNES('V<3)$',IPAK,3) 

CALL  LEGENOUPA*  ,3, 4. 0,2.0) 

CALL  ENOPL(O) 

1148  CONTINUE 

CALL  1NTAXS 
CALL  Y  A  X  AN6 ( 0  •  ) 

CALL  S I PPL  X 

CALL  TITLEC'TARGET  GROUND  PLOT  S', 100, 

*'  OFF  RANGE  <  2 )  -  YDS.*', 100, 

•'  DOWN  RANGE  Of)  -  YDS. S', 100, 6. ,8.) 

CALL  XTIC*S(5) 

CALL  YTICHS(5) 

X  0  R  G  *  ARMlN ( ZTP ,NP) 

X  M  A  X  G  =  ARM  AX (ZTP.NP) 

YMAXG*ARMAX(XTP,NP) 

CALL  GRAF (XORG, 'SCALE'. XMAXG, 

•  ARNlNfXTP  ,NP) , 'SCALE' ,YPAX6) 

NPL  S  *  5 

CALL  CURVE <ZTP,XTP.NP,NPLS) 

C  DRAW  ♦  AT  RADAR  LOCATION,  XT  ®R  T  ,  ZT=0. 

CALL  MARKERf?) 

CALL  SCLP1C(2.) 

RORY=Q. 

IFfRORX.LE . YMAXG)  CALL  CURVE (RDR Y  ,RDRX , 1 , 1 > 

CALL  RESET('SCLPIC') 

CALL  RESET('MARXER') 

CALL  PESSAGC'COV  0  *S', 100.1.,  8.9) 

CALL  RE  ALNO (  0 ,0 , 'ABUT ',' ABUT '  ) 

CALL  NESSA6 ( '  COV  R  *S',100,  ' AB UT ' , ' AB UT' ) 

CALL  RE ALNO (R( 1,1 ), 2, 'ABUT', 'ABU*') 

CALL  MESSAGf'  A  *S',  100.  ' AB UT ' , ' A BUT ' ) 

CALL  iNTNOfNRUN, 'ABUT', 'ABUT') 

CALL  DATE(UHEN) 

CALL  TOFDAY(TMEOO) 

CALL  MESSAG(WHEN,10,1.,V.3) 


call  messag(tmeoo ,  10, 'abut' , 'abut') 

CALL  EMOPl(0) 

116  continue 

lie  CONTINUE 
115  CONTINUE 

I E ( 1LAST.NE  .1  )  GO  TO  10 
CALL  DONE  PL 

120  FORMAT ( 1 X, 1  A, 1X,F6.1,3(F6.2),6(F9.3).£(F 7.3 )> 
130  FORMAT  (IX, 13,  4(F1P.1),7(F10.3>> 

133  FORMAT (1x,l4F9,2) 

150  FORMAT  () 

END 
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subroutine  intcon 

C  INTCON  IN1TILIZES  TRAJECTORY  VARIABLES 
DIMENSION  PAUV<3> 

CONNON  XT,YT,ZTtVXT,VYT,VZT,VEL0C.T1ANG,T2AN6.DELR.IG,TRATl« 
.DELT.NPRF ,TRAT?,TI.NPH,R9U,IG2,1CTS  ,PI ,IR, IH.IOS 
COMNON/INTC/  AZ .EL.ROLl .ROL2 ,ROL*  ,  AZDE6 ,ELD£6 .R0L1D6  .ROL2D6, 
•R0LAD6  ,T6S1 ,T6S2,RT.RD0TT,R2d0TT,  D T  ,  Tfi A D 1 , TR A D 2 , R AN6T , 

*RAN6TM,XR,YRtZR.VXTN,VYTN,VZTH.VR2D,AXT,AYT,AZT,AXT6,AYTG.AZT6. 
•  ANA , ANE  ,PAU V 
I  T  C  S  *  1 
AZ-O. 

£  L  *0  • 

ROL 1 *0 • 

R0L2*0. 

R0LA=0. 

AZDE6*0. 

ELDE6*0. 

R0LlD6*0. 

ROL2D6*0. 

T1AN6*0.  ; 

16*0 
162*0 
D  £  L  T*DT 
N  P  R  E  *  1 

C  3-D  TAR6ET  TRAJECTORY  DATA 
C  ORIGIN  AT  INITIAL  TARGET  POSITION 
C  RT  *  TRUE  RAN6E  RELATIVE  TO  RADAR 
C  RDOTT  *  TRUE  VELOCITY  RELATIVE  TO  RADAR 
C  R2D0TT  *  TRUE  ACCELERATION  RELATIVE  TO  RADAR 
IEUR.EQ.1)  RT*11275. 

IF(IR.£0.2>  R  T* 1 000 . 

IFIIR.E0.3)  RT*15Q. 

IF(IOS.EO.I)  R00TT*-1 75 • 

IFdOS.ER.2)  RDOTT*  — 100  •’ 

R  2  DOTT  *0 ■ 

C  VELOC  *  TARGET  VELOCITY  RELATIVE  TO  ORIGIN 
VELOC=-RDOTT 

C  TRAD-  *  TURN  RADIUS  OF  TGS-  6.  TURN 

TRA01*VELOC«VEL0C/T651*10.7252 
TRAD2*VELOC*VELOC/T6S2*10 .7252 
C  TRAT-  *  TURN  RATE  OF  TGS-  G.  TURN  (RAD/SEC) 

TRAT1*TGS1*10.7252/VEL0C 
TRAT2=T6S2*10.7252/VELOC 
R  A  N6T  *R  T 

RAN6TN*RANGT-«RD0TT*DELT*FL0AT(NPRF )) 

C  -R  *  RADAR  LOCATION  AT  T(0) 

X  R  *R  T 

V  R  *0  • 

ZR*0. 

C  -T  *  TRUE  X,Y,Z  TAR6ET  POSITION 

X  T  *0  • 

YT*0. 

Z  T  =  0  . 

C  V-T  *  TRUE  X.Y.Z  VELOCITY 

VXT  =  VELOC 

V  Y  T  *0 • 

V  Z  T  *0  • 

VXTN*V»  T 
VYTP*V Y  T 
VZTP*VZ  T 
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vh?&=rdott 

c  A-T  =  TRUf  X.Y.2  ACCELERATION 

AxT=0. 

A  Y  T  =  0  » 

A  2  T  =  0  • 

C  A-Tfc  =  TRUE  X , Y  ,  2  ACCELERATION  (G'S) 

A  X  T6  =0 • 

A YTG=0. 

A 2  T6®  0 • 

C  ANA  s  VERTICAL  ( A 2  )  acceleration  relative  to  target  orientation 
C  ANE  *  H0RI20NTAL  (EL)  ACCELERATION  RELATIVE  TO  TAR6ET  OR  I FNT  AT  ION 
AN A  *0 • 

ANE  =0 . 

C  PAUV(I)  »  PITCH  AXIS  UNIT  VECTOR 
C  PAUV(I)  *  X 
C  PA  LI  V  (  2  )  »  2 
C  P  A  U  V  (  3  )  *  Y 

PAUV(1)=0. 

PAUV(2) =1 . 

PAUV(3)=0. 

ENO 
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C  MANVR  CALCULATES  TRUE  POSITIONS  (X T * Y T , Z T »  AND  TRUE  VELOCITIES  (VXT, 
C  VYT.VZT)  TARGET  STARTS  3  ORIGIN  *  T»C,  AND  ELlfS  TOWARD  R  *0 Afi  FIXED 
C  LOCATION  RT  ON  X  AXIS 

C  X  =  DOWN  RANGE,  REFERENCED  TO  TARGET  3  T  - C 

C  y  =  UP  RANGE,  REFERENCED  TO  TARGET  3  T  r c 

C  2  -  OFF  RANGE,  REFERENCED  TO  TARGET  3  T  :0 

SUBROUTINE  PANVR 

COMMON  XT  ,YT  ,ZT  ,  V  X  T  ,  V  Y  T  ,  V  Z  T  ,  V  E  LOC  ,  T  1 A  N6  ,  T2  A  NC- ,  DELR  ,  IG  ,  TR  AT  !  , 
.0£LT,NPRF,TR AT?, II,NPH,k9C, I&2,ICTS,PI,IP,IH,I0S 
COMMON/  IN  TC/  A7,EL.R0L1,R0L2,R0L'*,A2DEG,EL0EG,P0L  IDG  ,R0L2DG, 
•R0L4DG  ,TGS1  ,TGS2,RT ,RC0TT,R200TT,  D T , TP A 0  1 , TR A02  ,  » *NG T  , 

*RANGTM,XR,YR,ZR ,VXTM,VYTM,V2TM,VP2P,AXT, AYT,A2T, »XTG, AYTG,A2TG, 
•ANA  ,  ANE ,P AUV 

C  *PGL4  ALONG  X-AXIS  ROTATES  PITCH  VECTOR  CLOCKWISE.  IFROM  X, Y,2  = 

C  L  ,  G  ,  1  TO  0 , - A , B  I 

C  RR  -  ROLL  RATE  OF  AIRCRAFT  (DEG/SEC > 

C  RRR  :  ROLL  RATE  OF  AIRCRAFT  (PAD/SEC) 

C  ICTS  =  COORDINATED  TURN  SWITCH 
DT=DELT 
RR  -  8S  . 

PRR=RR*PI/18C, 

CONDITIONAL 
.  JII.LT.NPH) 

C  .  .  INITIAL  TRAJECTORY 

.  ITCSrO 

XT=XT*CELR 

•MEASURED*  AIRCRAFT  POLL,  PRECE  E  D I N6  MANUVFR 
ROL4  =  ROL4MAR«*OT  ) 

..FIN 

T1ANG.LT  .R9Q | 

SECONO  TRAJECTORY 
R0L4=P0L4 ♦ l RPR*OT ) 

IF  (R0L4.GT • J  .55) 

.  fi  OL  4 ; 1 
...FIN 

IF ( IG.GT. 1« ) 

.  ICTS=0 

.  'MEASURED*  AIRCRAFT  ROLL,  PPECFEDING  MANUVE* 

,  R0L4=R0L4-tRRR*DT> 

..  .FIN 
I G  :  I G  ♦  1 

TIANG  =  AZ  ANGLE  OF  TARGET 
TiaNG=TRaT!»DELT*FLO*TJNPRF)*IG 
XT=XT^CELR*C0S«T1ANC» 

ZT=ZT*DELfi*SlN«nANG) 

3R  90  VXT  :  0,  VZT  =  VCLOC 
VXT=V£LOC*CCS  JT1 ANG) 

VZT  =VELOC*S  IN  »  T  1  a  f.G  ) 

.  .FiN 
«  .TRUE  .  ) 


c 

c 


.  .  ThI^C  TRAJECTORY 

.  .  *l*t  i  SUf  LT  *  AlfiC^AF  T  P  CL  L 

.  .  IF  »K0L4.C-T.l'.D) 

.  .  *  ROL^PCLW-lPSKiDT  > 

•  •  •••FIN  \ 

.  .  IFlKOl<J.LT.w.i.f. 

•  •  •  ROL<*  =  r.o 

. FIN 

.  .  Ib2=IG?*l  \ 

.  .  T..AN6  =  T8AT2*PtlT*F‘I.0Arf  NPRF>*IG 

.  .  YT=YW.'ELR*SlN<7?Af\G> 

.  .  2T=2T*G£LP*COSU2ANl&) 

•  .  VYT=VELOC*S IN ( T2ANGl\ 

.  .  V2T=VELOC*COStTLANGF\ 

.  ...FIN  \ 

...FIN  \ 

RETURN  \ 

END  \ 
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C  SUPROUT 1NE  f  Tk  AL 1 

C-F  TKALi - S - 

C  * 
f 

C  FUNCTION 
C  USAGE 
C 

C  PARAMETERS  K 

C 

C 

C 

C 

C  X 

C 
C 
C 

C  M 

C 

C  G 

C  t 

C 

C  S 

c  a 

c 

c  R 

c 

C  P 

c 

c 

c 

C  IN 

c 

c  IS 

c 

C  IL 

c 

C  N 

c 

C  Ml 

c 

C  L 

c 

C  TT 

c 

C  T? 

c 

C  IT 

C 

C  T  3 

C  TT 

C  IER 

C 

c 

c 

c 


<K,X,H,G*Y,S*tt»R,P,IN,lS,lL,N,«1,L,T1,T2, 


IT.T3.TT.IER) 

-  ITERATED  KALMAN  FILTERING 

•  CALL  FTkAIEU  ,X,H,6.Y,S,e,R,P,!N,IS,lL.N.Ml,t 

T1,T2,IT,T3,TX,IER> 

-  input  step  counter*  k*o.i,2»...  when  k«0. 

VECTOR  X  SHOULD  CONTAIN  THE  PRIOR  ESTIMATE 
OF  THE  MEAN  OF  X*  AND  THE  PROGRAM 
CALCULATES  THE  ESTIMATED  VARIANCE  OF 
X  AS  P=606“TRANSP0SE  AT  STEP  0. 

-  INPUT/OUTPUT  VECTOR  OF  LENGTH  N.  ON  INPUT* 

X  IS  THE  STATE  VECTOR  AT  STEP  K  *  AND  ON 
OUTPUT*  X  CONTAINS  THE  ESTIMATED  STATE 
VECTOR  AT  STEP  K*1. 

-  input  matrix  of  dimension  n  by  n.  h  is  the 

transition  matrix  at  step  k* 

•  INPUT  MATRIX  OF  DIMENSION  N  BY  L  AT  STEP  K • 

-  INPUT  OBSERVATION  VECTOR  OF  LENGTH  Ml  AT 

STEP  K  ♦  1  • 

•  INPUT  matrix  of  dimension  mi  by  N  at  step  K*1 

-  INPUT  COVARIANCE  MATRIX  OF  U*  OF  DIMENSION  L 

AT  STEP  K. 

-  INPUT  COVARIANCE  MATRIX  OF  V*  OF  DIMENSION 

Ml  BY  Ml  AT  STEP  Ml, 

-  INPUT/OUTPUT  MATRIX  OF  DIMENSION  N  BY  N. 

ON  INPUT.  P  IS  THE  VARIANCE  MATRIX  OF  X 
AT  STEP  K.  ON  OUTPUT,  P  IS  THE  ESTIMATED 
VARIANCE  MATRIX  OF  X  AT  STEP  K  +  1. 

-  FIRST  DIMENSION  OF  H  *6 ,  AND  P  AS  DIMENSIONED 

IN  CALLING  PROGRAM. 

>  FIRST  DIMENSION  OF  S  AND  R  AS  DIMENSIONED  IN 
CALLING  PROGRAM. 

-  FIRST  DIMENSION  OF  0  AS  DIMENSIONED  IN 

CALLIN6  PROGRAM. 

•  INPUT.  SEE  DESCRIPTIONS  OF  X.H,G*M,P. 

N  MUST  BE  GREATER  THAN  0. 

-  INPUT.  SEE  DESCRIPTIONS  OF  Y.S.R.T3. 

Ml  MUST  BE  GREATER  THAN  0. 

•  INPUT.  SEE  DESCRIPTIONS  OF  G.Q. 

L  MUST  BE  GREATER  THAN  0. 

-  WORK  MATRIX  OF  dimension  NM  BY  NM*  where 

NM  IS  THE  MAXIMUM  OF  N  AND  Ml. 

-  WORK  MATRIX  OF  DIMENSION  NM  BY  NML.  WHERE 

NML  IS  THE  MAXIMUM  OF  NM  AND  L. 

-  FIRST  DIMENSION  OF  T1  ANO  T2  AS 

DIMENSIONED  IN  CALL1N6  PROGRAM. 

-  WORK  VECTOR  OF  LENGTH  Ml.  j 

-  INPUT.  ITERATED  FXTENDED  THRESHOLD  j 

-  ERROR  PARAMETER.  IER  *  0  IMPLIES  NO  ERROR. 
TERMINAL  ERROR  *  12t*+N 

N  «  1  INDICATES  ONF  OF  IN,  IS*  IL,  OR  IT 
IS  TOO  SMALL.  OR  THAT  ONE  OF  N.  Ml,  j 

OR  L  IS  NOT  A  POSITIVE  1NTE6ER . 


N  »  2  INDICATES  “ATM*  INVERSION  FAILED. 
PRECISION  -  SINGLE 

REO'D  !*SL  ROUTINES  -  L E 0 T 1 F . LUO A T F ,1 Uf L * F , UF R T S T  , V“UL F F , VMUL F P 
LANGUAGE  -  FORTRAN 


C  LATEST  REVISION  -  NAT  10.  1*79 

C  “ODEL 

C  X(K*1)  «  H(K)M(t)»G(K)tU(K) 

C  V  (  X  ♦  1  )  *  S(K+1)*X(K*1)*V(K«1) 

C 

SUBROUTINE' FTKALI  <K,X,H,G,V,S.Q,R,P,1N,1S,1L, 

*  N.NI.L.TI.TZ.IT.TT.TX.FG.IER.IPS) 

c 

DIMENSION  X(1>,H(1N»1),G(IN»1),V(1)»$(IS»1)»G(1L*1)» 

•  «(!S.  1).  PUN,  1),TT<IT,1),T2(IT.1).T3(1) 

DIMENSION  Xk ( 20) «  XP<20),fcX (20) ,TX< IN) ,T4<20) ,HNL<2C>  , 

*EXM(20) ,F6C IT.1 ) 

DIMENSION  T5(20).T6(10»10> 

DATA  10/0/, 11/1/, 120/20/ 

IF  (IN.GE.N  .AND.  IS.6E.M1  .AND.  1L.GE.L 

*  .AND.  (IT.6E.N  .OR.  IT.GE.Ml)  .AND.  N.GT.O 

•  .ANO.  M1.GT.0  .AND.  L.GT.O)  60  TO  5 
I E R  *  129 

GO  TO  9000 
5  I E  R  *  0 

C  CALCULATE  P  IF  K  *  ZERO 

C  P(0)  «  G(0)*fi(Q)*GT(0) 

IF  (K  .NE.  0)  60  TO  TO 

CALL  VMULFF  (  6,  Q,  N,  L.  L  •  I N, I L ,T 2 , 1 T , I E R ) 

CALL  VMULFP  (T2,  G,  N»  L ,  N.IT.1N,  P.lN.IEB) 

IF (IPS.EQ.O)  GO  TO  166 

WRITE (6,160)  (P(1,J),J«1,IN),(TX(I),Ib1,IN) 

DO  165  I b2 , I N 

WR I TE (6 , 1 61 )  (P(I,J),Jb1,IN) 

165  CONTINUE 

166  CONTINUE 

160  F ORMAT ( *  P(0)  *',10(E10.4)) 

161  F0RMAT(8X, 1U<E10«4>> 

10  CONTINUE 

C  SAVE  INITIAL  X 
00  12  I  =  1  ,  N 
XK(I)sXU) 

12  CONTINUE 

C  CALCULATE  X-PRIME  AT  STEP  K*1 

C  *'«*1)  »  H<IC>«X-HAT(K) 

CALL  VMULFF  (H,  X,  N,  N , 1 1 , I N , I N • T1 , I T , I E R > 

DO  15  I  »  1 , N 

XP(I)  *  T 1  (  I  ,  1  ) 

X (1 >*XP< I ) 

15  CONTINUE 

IF(IPS.EQ.I)  WR1TE(6,17G)  ( XP C 1 )  .  I* 1 ,N) 

170  FO»MAT('  X-PRIME  s  H.X  *  '  ,  1  0  (  F  1  0  •  2  >  > 

C  CALCULATE  P-PRJME  AT  STEP  K*1 

C  P  *  (  A  ♦  1  )  •  H(K>*P(K)bhT(k)«G(k)*0(K)*6T(K> 

CALL  VMULFF  (  H,  P.  Nv  N,  N , I N , IN  ,T 1 . I T , I E R ) 


CALL  VttULfP  <T1,  H,  At*  N «  N,IT,1N,  P.1N.IER) 

CALL  V*UL  F f  (  6*  0.  N,  L.  L » IN, IL *T 2 , 1 T , I E R ) 

CALL  VMULFP  <T2  »  6,  N,  L.  N *  IT* IN *T1 , IT ( I£R) 

00  25  1  *  1  » N 
00  20  J  3  1 »N 

P(I,J)  3  P(lii)  ♦  T 1 (  I  *  J  ) 

20  CONTINUE 
25  CONTINUE 

lF(IPS.EO.O)  60  TO  186 
WRITE  16,180)  (P(1,J)*J*1.IN) 

00  185  1 *2  *  IN 

WRITE (6*181 )  (P(I » J ) * J  =  1 *  IN) 

195  CONTINUE 

186  CONTINUE 

180  fORNAT ( '  P-PAIME  « ' , 10 ( E 1 0 .4 > ) 

181  F0RMAT(11X.10(F10.4)) 

C  CALCULATE  MATRIX  K  AT  STEP  K«1 

C  n(K*1)  *  P#CX*1)*ST«*1)*<SCIC+1)*P‘'<K41)*ST<X^1>*RCI{*1))**-1 
CALL  VMULFP  C  S,  P.M1,  N,  N  *  I S • IN ,T 2 , IT  ,  I £ R ) 

IP ( IPS. £8.0)  60  TO  28 
00  27  1*1  *  M 1 

WRITE (6*187)  I,(T2(I,JJ),JJ*1,M1) 

27  CONTINUE 

28  CONTINUE 

CALL  VMULFP  (T2  *  S  »  Ml »  N * Ml  ,  I T , 1 S  ,T 1 , I T *  I E R ) 

DO  35  I  *  1,"1 
00  30  J  *  1 « Ml 

T1 (  I  » J  >  *  T 1 ( I  *  J )  ♦  R  (  J  *  I ) 

30  CONTINUE 

IF(IPS.EQ.I)  WRITE(6*18?>  I  * ( T1 ( 1  * J J > * J J 3 1 *M1 > 

35  CONTINUE 

187  FORMAT ( 0  S.P*ST4R('.I1  ,',))  =  T1  * ' , 10 ( F 10 .2 ) > 

CALL  LE0T1 F (T1 »  N * M 1 , I T *T 2 , 10 *T 3 , I E R > 

IF  (IER  .EQ.  0)  60  TO  AO 
I E  R  *  130 
60  TO  9000 
AO  DO  50  I  *  1,M1 

00  AS  J  *  1 *N 

T 1  <  J  «  I  )  *  T2(I,J) 

F6(J*I>*T1(J*I) 

A 5  CONTINUE 
50  CONTINUE 

IP(IPS.EO.O)  60  TO  194 
WRITE<6,190)  (T1 ( 1 » I  )  *  I* 1  * Ml  ) 

00  193  1*2. IN 

WRITE  (6*191)  (T1(I,J),J«1,W1> 

193  CONTINUE 
19A  CONTINUE 

190  FORMAT ( "  K  *  T1  *',10( F10.2)) 

191  FORMAT(10X,10(P10.2)> 

C  CALCULATE  X-MAT  AT  STEP 

C  KALMAN  ESTIMATE 

C  X-HAT (K+1)  «  X'(K)-K(K^1)*(S(K41)*X'(K)-T(K41)> 

K  L  *0 

00  53  1*1  . IN 
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EX<D*0. 
f XM(I)*1F9 
51  CONTINUE 

54  CONTINUE 

00  542  1*1  , IN 

T4(I>*XP< I)-*(I) 

542  CONTINUE 

C  OBS.  ERA.  *  S <X'-X )-T*HNL  *  T5 

55  CALL  VMULFF  (  S,  X,M1,  N  .  1 1  , I S , IN ,T 3 • 1 S  ,  I E R  ) 

CALL  VMULFF  <  S,T4,Wl,  N , 1 1  ,  I S  •  I N  ,  T 5 , 1 S  ,  I E R  ) 

CALL  VflULFM  S.  X,*1,  N , I 1 . I S , I N , HNL , I ?C , I E R ) 

00  60  1  *  1,M1 

T3  ( I  )  =  T  3  <  I  )  -  V  ( 1 ) 

60  CONTINUE 

IF(IPS.EO.I)  «RITE(6,195>  (  T  3  (  I  >  ,  I  *  1 .  PI  >  ,  (  X  <  I  >  ,  1  *1  ,N  ) 
195  FORMAT  <  *  OBS  ERR  *  S.X-V  *#,12(F9.2)) 

C  T1  *  6  A I N  K 

CALL  VMULFF  (T1.T3,  N ,M1 ,  II , IT , I S ,T2  ,  I T *  I E R ) 

00  65  I  *  1  ,N 

X  (  1  >  *  XPCD  -  T2<  1.1) 

T6(I.1)*T2(I,1) 

65  CONTINUE 

IF(IPS.EQ.I)  WRITE (6.200)  ( X C I) , I *1 ,N ) , ( T2 ( 1 , 1 ) . I « 1 , N ) 
200  FORMAT ( *  X-MAT  *' . 1 0 C F10 . 2 )  ) 

69  CONTINUE 

C  CALCULATE  P  AT  STEP  K*1 

C  PU*1>  *  P'(K*1)-MK  +  1)*S(K*1)*P'(lCd> 

CALL  VMULFF  (  T 1 ,  S,  N,M1,  N  .  IT . I S  ,  T 2 , l T , I E R ) 

CALL  VMULFF  (T2,  P.  N.  N.  N  ,  I T , I N  ,T 1 , I T , 1  £ R ) 

00  75  1  *  1 , N 
T2CI,1)*T6CI,1) 

DO  70  J  *  1.N 

P(I,J)  *  P(I.J>  -  T1C1.J) 

70  CONTINUE 
75  CONTINUE 

IFC1PS.E9.0)  60  TO  216 
WRITE  (6,210)  (P(1,J).J*1,IN) 

DO  215  1*2. IN 

WR1TEC6.211  )  (P< I . J )  ,  J  =  1  ,  IN) 

?1 5  CONTINUE 
216  CONTINUE 
60  TO  9005 
9000  CONTINUE 

CALL  UERTST  ( I ER  ,  6HFTKALM  ) 

9005  RETURN 

120  FORM AT (IX,  14,  IX,  F6.1.10(F11.2>) 

130  F0RWAT(1X,I3.12<F10.3)> 

140  F0RPAT(2X,I3,1?(F10.?)> 

210  FORMAT  < '  P  *',10<F10.2>) 

211  FORMAT (  5X , 10( F10.2)  ) 

END 
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